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Simulation studies for both a 12,000 pound vehicle and the 
435 pound testbed vehicle, designed and built at the School 
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Figure 5.18 NPS AUV II - Pitch angle using alternate 
forward velocity estimation model 
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I. INTRODUCTION 


A. GENERAL 

The United States Navy now uses Unmanned Underwater 
Vehicles (UUVs) for performing a variety of missions [Ref. 
l:pp. 60-88]. Currently, these vehicles are tethered and 
are controlled by data links to human operators. Their 
small size and corresponding ability to go where manned 
vehicles cannot go are their primary advantages. These 
advantages also apply to Autonomous Underwater Vehicles 
(AUVs). An Autonomous Underwater Vehicle is a type of UUV 
that is not limited by the need for local human control. 
The freedom from requiring an external control interface 
theoretically allows this type of vehicle to perform a 
greater range of missions than its tethered counterpart 
[Ref. 2:pp. 571-575]. 

While autonomy has clear advantages, it does require a 
sophisticated level of on board processing ability. The 
organization for the control of the vehicle can be broken 
into a hierarchy of levels in which the vehicle senses, 
BEnks and acts [Ref. 3:pp. 1-3]. At any of the levels the 
vehicle requires an interface to its real world environment 


wm order to function properly. 


In addition to the obvious need for information 
concerning the physical surroundings of the vehicle, such 
as obstacles, data needed at any of these levels will likely 
include the current vehicle location. Additionally, the 
vehicle will need to have its present state, in terms of 
velocities, angle rates, and attitude available for guidance 
and control. An on board navigator is designed to supply 


this information. 


B. AIM OF THE STUDY 

This thesis 1s concerned with the navigation problem of 
an AUV. As a result of the method chosen for addressing 
this problem, this thesis is also concerned with the 
guidance and control functions of the vehicle since these 
functions are assumed to require signals fed back by the 
navigator. 

Due to the unavailability of radio navigation aids (such 
as LORAN, OMEGA, or GPS) in the underwater environment, the 
navigation system of the AUV is primarily based on inertial 
measurements. For reasons of covertness, the vehicle is not 
expected to broadcast its position, so it cannot rely on 
navigational processing by a mothership and thus the 
navigator must be self-contained. 

Generally, inertial navigation systems (INS) are 
complicated and historically have relied on expensive 


gyroscopically stabilized platforms, known as gimballed 


Systems. A triad of accelerometers is placed on the stable 
platform to measure rectilinear accelerations in the 
Platform pi e. s inertiabs coorainmnatess[Ref. 4:pp. 85-86). 
Generally, the platform could be stabilized to represent any 
coordinate system. Operating inertial navigation systems 
have been built in which the platforms are stable relative 
to the stars, to a non-rotating earth coordinate system, and 
to a locally level coordinate system, among others. In any 
case, the accelerations are measured in the inertial 
coordinate system, via the stabilized platform. These 
measurements are then properly integrated to obtain a 
position estimate. [Ref. 4:pp. 193-223] 

Until the last 20 years, limitations in computer speed 
and physical size, as well as computer memory cost have 
prevented designers from using strapped-down systems. A 
strapped-down INS is similar to a gimballed INS as described 
above except that the inertial reference coordinate system 
is stored in computer memory rather ina stable platforn, 
cuiamene accelerometer triad 1s rigidly attached to the 
vehicle. The motion of the vehicle relative to the chosen 
inertial coordinate system is determined by combining 
measurements from rate gyroscopes and accelerometers. The 
angular rate information from the gyroscopes is transformed 
and integrated to obtain the vehicle’s attitude in the 
chosen coordinate system and the accelerations are 


integrated and transformed using this attitude information 


to give vehicle position in the inertial frame [Ref. 5:p. 
38]. 

Until microprocessors were developed, the amount of 
computing power required to perform these operations was 
beyond the capability of any on board computer. However, 
with the advent of VLSI CMOS technology, processors are 
small enough and memory is inexpensive enough to make the 
system feasible and such systems are in use. 

For any navigation system to be useful, the accuracy of 
the instruments has to be such that the navigator's error is 
within the required tolerance of the vehicle which relies on 
the navigation system. However, accurate sensors tend to be 
large and expensive. It is the aim of this thesis to study 
the feasibility of combining measurements from inexpensive 
instruments on board an AUV to generate relatively good 
position estimates over a short time interval. 

To study the feasibility of operating an AUV, the Naval 
Postgraduate School has designed and built a testbed AUV, 
known as NPS AUV II. It is for this vehicle that this 


thesis is designed. 


C. METHOD OF APPROACH 

This thesis is concerned with the short-range navigation 
problem for the NPS testbed AUV. Because this vehicle is 
small it cannot carry the stabilized platform required of a 


gimballed INS. It must therefore rely on a strapped-down 


System. Currently however, there is no commercially 
available strapped-down system which can be incorporated 
into the AUV. It is therefore necessary to design a 
Specific navigator for the AUV. 

Being limited in complexity and cost, the navigator has 
to rely on previously purchased and installed instruments. 
Also, because the system need only operate over short ranges 
the effects of the earth’s orbit and rotation can be 
neglected. This approximation also has the effect of 
Simplifying the navigator. 

The approach taken in this thesis to solve the 
navigation problem is to use a nonlinear dynamic model of 
the submerged vehicle to filter gyroscope and accelerometer 
readings to estimate the vehicle’s position as well as the 
vehicle’s attitude, velocity and rotational rates. This 
approach is diagramed in Figure 1.1. 

The thesis is presented in five parts. Chapter II 
discusses some background and the theory behind the 
navigator design. Chapter III discusses the details of the 
design and the techniques used in simulation. In Chapter IV 
we present the results obtained from the simulation study, 
while Chapter V shows the results obtained using recorded 
sensor readings taken from the Naval Postgraduate School's 
vehicle. Finally, Chapter VI summarizes the the results of 
this research and contains conclusions and recommendations 


Lor application and further study. 
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Figure 1.1 Navigator Concept 


II. LINEARIZATION AND EXTENDED KALMAN FILTERING 


A. GENERAL 

This chapter discusses the general theory used to design 
the AUV navigators. A brief introductory presentation is 
given for each of the concepts used in the development of 
this work. None of the concepts is presented in an 
exhaustive manner and derivations are not provided. A prior 
understanding of these concepts is assumed; therefore, only 
a brief review is presented for clarity. For more detailed 
explanations and developments, the reader is directed to the 


References listed herein. 


B. LINEAR MODELING 

Fil Control theory we encounter the problem o£ 
representing a dynamic physical system mathematically. A 
common way of doing this is to use a state-space model by 
breaking the Nth- order differential equation representing 
the system into N, coupled, first-order differential 
equations. These equations can be easily represented using 
matrix algebra and matrix notation as long as the system 
being modeled is linear. A linear system exhibits the 
properties of both homogeneity and superposition [Ref. 6:pp. 


14-25]. 


A linear, time-invariant (LTI), state-space 


representation of a continuous-time system is given by 


k(t) = Ax(t) + Bu(t) (2.1) 


where x is the system state vector, u is the input vector 
and A and B are the state transition and input matrices 
respectively. 

In the time-invariant case, both x and u are functions 
of time, while A and B are not. If A and B were functions 
of time, then the system would be classified as time- 
varying. 

Generally, this description is supplemented with a 
measurement equation which represents the output of the 
system as a function of the states and the inputs. Again, 
if the system is linear, matrix notation may be used, and 


the measurement is given by 


y(t) = Cx(t) +Du (t) (2.2) 


where y is the system output. Generally, there is more than 


one output so y is a vector. 


A system can be represented in discrete time with 


analogous equations. These are: 
MS D x(Kk) +F u (k) (2.3) 
y(k) = Cx(k) +Du(k) (2.4) 


where k is the time index. 

Knowledge of the system state at all times is often 
required to control the system. However, it may not be 
feasible to measure all of the states of a system because 
systems can be complex and putting instrumentation in place 
to measure the states may not be possible. In such cases, a 
filter known as an observer is used. (Ref. 6:p. 259] 

An observer is a dynamic subsystem based on a model of 
the system being observed which recursively predicts the 
System state. The error between the output of the observer 
and the output of the system is driven to zero by correcting 
the state prediction with the output error signal [Ref 6:p. 


262). This relationship is given by 
X(k+1) = OX(k) +Pu(k) +Gl(y(k) -CX(k)) (2.5) 


where G is the observer feedback gain, and £ is the 
observer, or estimated state. 

The result is that an estimate of the system state is 
made available without directly measuring all the states. 


Because this thesis is written using discrete-time models 


and filters, this equation is given in its discrete-time 
form. 

In the presence of noise, which may include unknown 
disturbances perturbing the system dynamics, known as plant 
noise, or noise in the instruments which measure the output 
of the system, known as measurement noise, the observer 
feedback gain becomes an important factor in minimizing the 
sensitivity of the overall system to the noise disturbances. 
The Kalman filter is an observer which provides optimal 
state estimates for linear systems in the presence of noise. 
The Kalman filter is based on the assumption that the plant 
noise and the meaSurement noise are independent, white, 


Gaussian processes, so the system equations become 


x(k+1) = Ox(k) +TP, u(x) +) w(k) 
(2.6) 


Gg (x) -Piu (k) + (K) 


y (x) 
where w is the plant noise vector, and v is the measurement 
noise vector. 

The Kalman filter gain matrix is time-varying and is 
related to the plant noise and the measurement noise through 


their respective covariance matrices, Q and R, by 


P(k«1|k) = 6 P(X) &*- T, QT 
G(k+1) = P(k+1|k) C'LCP(k*1|X) CT * R7 (2.7) 
P(k+1|k+1) =[I-G(X-1) C]jP(X-1/|X) 


y» 


where P is the variance matrix of the estimated state. [Re 


6:pp. 411-417] 
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With these gain equations, the filter equations are: 


&(k+1|k) = ®§(k[k) +Tu(k) 
Y = CX(k+1|k) (2.8) 
EERE A) ales 1) (k1): 


This is known as the predictor-corrector form of the 
Kalman filter in which the next state is predicted using the 
state space model and the measurement is used to correct the 
Prediction. [Ref. 7:p. 3) 

The Q matrix describes the unknown noise that disturbs 
ene ssystem, while the T; matrix describes the way this noise 
enters the system. If Q is diagonal then the disturbances 
are assumed to be independent, otherwise they are correlated 
as described by the off-diagonal terms in Q. If Q has large 
values, then large deviations from the systems's predicted 
state will be expected and more reliance will be given to 
the measurements for those states which correspond to the 
terms in Q which are large. 

The R matrix describes the unknown noise that disturbs 
the sensors which return the measurements. The Kalman 
filter considers the measurement noise to enter all the 
individual measurements so there is no D, matrix. As with 
the measurement noise covariance matrix, if R is large, then 
the measurements are expected to deviate more from the 


states being measured, and the Kalman filter will rely more 


MT 


on the predicted state than on the measurements. [Ref. 8:pp. 
1290230532) 

The initial P matrix affects the reliance the Kalman 
filter has on the initial conditions. Large values in P(0) 
mean that the filter will not rely on the initial 
conditions, but will instead give more weight to the 
measurements. This allows the estimated state to change 
rapidly as the filter goes through its transient stage. In 
Steady state, however, P(k) has no effect on the Kalman 
filter because it approaches a constant value that is 
dependent only on the system and the noise covariance 
matrices. 

Because the gain equations do not depend directly on 
time or on the state trajectory in a LTI system, the gain 
can be calculated a priori and recalled from memory as 
needed. There is no need for real-time gain computation. 
Moreover, the gain matrix approaches a steady-state value 
which is determined by the system equations and the Q and R 
matrices through the associated Riccati equation. In many 
cases, using the steady- state gain matrix instead of the 
time-varying gain matrix gives satisfactory results in a 
reduced-complexity algorithm which does not take into 
account prior knowledge of the initial conditions. [Ref. 


8:pp. 238-244] 
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C.  LINEARIZATION 

Thus far the discussion has been limited to linear 
Systems; however, many systems in which the control engineer 
is < ccosted are nonlinear. Nonlinear system models are 
more general than linear models and can contain a wide of 
variety of nonlinear characteristics for which limited 
analytic tools exist. In general, nonlinear systems do not 
exhibit the properties of homogeneity or superposition, and 
may include transcendental, trigonometric or other nonlinear 
functions (Ref. 9:pp. 351-353]. Such is the case with the 
model chosen for this thesis which will be discussed in 
Chapter III. 

A method for working with nonlinear systems is to 
linearize them using a truncated Taylor series 
approximation. The Taylor series approximates a function 
around a given point as an infinite sum of weighted, 
analytically determined, partial derivatives. A general 


Taylor series around the point x, is given by 


f(x) = y SIUS) yr. (2.9) 
posse QD n 
A truncated Taylor series only uses a few of the terms 
of the sum. In order to realize a linear function from the 
Taylor series expansion of a nonlinear one, the series must 
be truncated at the n = 1 term. This method is general and 


can be applied to any nonlinear system, and it will be valid 
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in a region surrounding the linearization points REF 10408 
3] 

For the case of a discrete-time dynamic system 
represented in state space, which, in general, is not a 
function of a single variable, but is rather a function of 
time, the input vector, and the past state vector, the 
Taylor series is defined over the partial derivatives of 
each of the independent variables. In the case of a time- 
invariant system, the series is expanded about an operating 
point described by the state, x,, and a corresponding input, 
ug. A nonlinear state space equation can then be 


approximated as 


! CEA keen) df (x ., u.) 
x= £(%, 14.) == Ni = 
x 


l 2 2 (u-u.) (2.10) 


where f is the nonlinear system function. This notation 
implies that the partial derivatives are constant terms, 
calculated analytically and evaluated at x,, and ug. 


The state, x,, and input, ug, around which the system 
is linearized, might not be constant. Most of the time, the 
System is linearized around the trajectories x. (t), and 


u (t). Because 


A E (< (RACE) ) (2.11) 
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Equation 2.10 can be rewritten as 


of (x) of (x m 
A) S NA E + SS Amb) (2 12) 
Ox Qu 
where 
AS AAA 


Au tu stt) u (t) 


so that the partial derivatives which form the right-hand 
side of Equation 2.12 are analogous to the A and B matrices 
of Equation 2.1. 

Just as a nonlinear system equation can be expanded 
using a Taylor series, a nonlinear measurement equation can 
also be expanded and linearized. The formulation for this 
expansion is similar to the above expression except that the 
nonlinear measurement equation is used to generate the 


analogs of the C and D matrices. 


D. EXTENDED KALMAN FILTER 

The Kalman filter is derived for linear systems with 
linear measurement equations; however, using the 
linearization techniques described in the last section, this 
filter can find application to general, nonlinear, systems. 
This is known as the Extended Kalman Filter. The Extended 


Kalman filter is suboptimal and may suffer convergence and 
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stability problems, but it has been shown to be useful ina 
variety of applications. [Ref 8:p. 189] 

The form of the Extended Kalman Filter equations are 
essentially similar to those of the linear Kalman filter. 
The nonlinear model is used to predict the state and the 
nonlinear measurement is used to correct the prediction. 
The most significant difference between the Extended Kalman 
Filter and the linear Kalman filter is in the gain 
equations. Rather than using the state transition, the 
input, and the measurement matrices to calculate the gain, 
the Extended Kalman filter uses the linearized model of the 
nonlinear system. It uses the partial derivatives of the 
nonlinear state equations and the nonlinear measurement 
equations. These partials are evaluated at each estimated 
state. As such, the gain, K, cannot be computed in advance 
because it is dependent on both the state trajectory and the 
input history. 

Calculating partial derivatives of the nonlinear system 
and measurement equations for each measurement as well as 
calculating the Kalman filter gain matrix is a computational 
burden. In order to overcome this problem, it may be 
possible to use gain matrices calculated in advance by 
choosing discrete points in the system's state space about 
which to linearize the nonlinear system.  Practically, only 
some of the system states are varied while others are kept 


constant. Using the chosen points, several linear 
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approximations to the nonlinear system are calculated and 
then used to calculate the steady-state Kalman filter gain 
matrix associated with those particular points. Once the 
gains are calculated, the Extended Kalman filter is 
implemented by determining which of the chosen linearization 
points is closest to the current estimated state and the 
corresponding gain matrix is used in the Extended Kalman 
filter equation given as the last of Equations 2.8. [Ref 
Bp. 189] 

In using the Extended Kalman filter, the nonlinearities 
guermodeled aseplant noise. This means that the TL, matrix 
is usually the identity matrix. Furthermore, because the 
nonlinear effects are not included in the gain equations, 
the Extended Kalman filter can be very sensitive to the Q 
and R matrices. Choosing improper values can make an 
Extended Kalman filter unstable. The values chosen for 
these matrices should not necessarily correspond to the 
actual noise expected in the system or in the measurements, 
but rather they must made large enough to provide a robust 


prediction in spite of the nonlinear effects. 
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III. INTEGRATED NAVIGATOR DESIGN 


A. GENERAL 

The design techniques used in this thesis are presented 
in this chapter. A brief description of the models used in 
the designs is given, as well as a summary of changes made 
to the models to facilitate their use in the navigator. An 
explanation of the measurements used to drive the navigator 
is also given. Finally, a description of the simulation 
environment, including an introduction to the use of MATLAB 
MEX files, as well as an outline of the navigator's program 


structure, is presented. 


B. USE OF NONLINEAR MEASUREMENT MODELS 

One of goals of this study has been the integration of 
nonlinear measurement models with the Extended Kalman filter 
navigator. The nonlinear measurements provide additional 
information about the vehicle’s state which improve the 
accuracy of the filter. 

In order to obtain more information about the vehicle’s 
velocity, depth rate is measured. Depth rate is related to 
both the pitch angle of the vehicle and its velocity as 
illustrated in Figure 3.1. This diagram shows only two 
dimensions for clarity; however, a discussion of the full, 


three-dimensional depth-rate equation follows in Section D. 


18 


body coordinate a om “ pitch angle 
vertical velocity E UR c: -8 


w 


body coordinate 
forward velocity 


u 


= -U SINB + W cose 





Figure 3.1 Simplified diagram of depth rate and velocity 
relationship 


The other nonlinear measurement model used in the 
navigator is for the accelerometers. Because accelerometers 
cannot distinguish between accelerations and gravitational 
forces, they can be used to measure gravity. In this case, 
they are used to provide a nonlinear measurement of the 
gravity vector in the vehicle coordinate system. This can 
be interpreted as a nonlinear measurement of the vehicle's 
attitude in earth coordinates. A simplified diagram of this 
concept is shown in Figure 3.2. A more detailed description 


of this measurement is given in Section D. 
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Figure 3.2 Simplified diagram of accelerometer measurement 


C. VEHICLE MODELS 
1. The Vehicles 

Two vehicles are studied in this thesis. One is a 
17.4 foot long, 12,000 pound Swimmer Delivery Vehicle (SDV), 
and the other is the 7 foot long, 435 pound testbed AUV 
designed and built at the Naval Postgraduate School known as 
the NPS AUV II. The SDV model was used in the preliminary 
stages of this work because an accurate model of the NPS AUV 
II was not yet available. 

The two vehicles are geometrically similar but there 
are minor differences. They both have a rectangular cross 
section rather than the usual body of revolution more 
typical of submarine vehicles. The SDV differs from the NPS 
AUV II in that it has a deep keel in which a third propeller 


is housed for surface operation. Although the model has 
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Peavyision for this feature, it is not used in the 
simulations. The NPS AUV II differs from the SDV in that it 
has a bow rudder which increases its maneuverability. This 
feature is included in the model and is used in the 
Simulations. 

2. The Models 

The models chosen for this work are based on 
modified equations of motion for submarine vehicles 
developed by Gertler and Hagen [Ref. 11]. As opposed to a 
typical inertial navigation system (INS), these models are 
representations of the vehicle dynamics rather than models 
of the sensors and of coordinate system relationships to 
inertial space. As such, no provision is made for the 
earth’s curvature, its rotation, its orbit around the sun, 
Or gravitational anomalies. This limits the applicability 
of the models to short-range missions. 

Both models are 12-state, six-degree-of-freedom, 
nonlinear models which are based on three specific force 
equations which govern linear accelerations in body 
coordinates, three specific torque equations which govern 
angular accelerations in body coordinates, and six kinematic 
relationships which translate linear and angular velocities 
from body fixed to inertial coordinates. The specific 
equations of motion are given in the NCSC report by Crane, 


Sumney and Smith (Ref. 12]. The two coordinate systems used 
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by the models are typical of those used to describe aircraft 
motion in that they are both right-handed with the vertical 
axis pointing down. In the reference system, down is in the 
direction of the gravity vector, and for the body coordinate 
system, down is through the bottom of the vehicle. These 


systems are shown in Figure 3.3. 





Figure 3.3 Diagram of coordinate systems 


a. Vehicle State 


As previously stated, these models have a 12- 


element state vector given by 


x=[uvwpqrXYz008Y7. (3.1) 


The first three elements of the state vector, u, 


v, and w, are the 3 mutually orthogonal velocities in 
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vehicle, or body coordinates. They are given in feet per 
second. 

The next three elements, p, q, and r are the 
three angular rates associated with vehicle motion around 
the vehicle coordinate axes. They are in units of radians 
per second. The directions for these six states are shown 
in Figure 3.1. 

X, Y and Z are the coordinates of the vehicle’s 
location in inertial space, and &, ©, and ¥ are the Euler 
angles which describe the vehicle’s attitude in inertial 
space. $ is the roll angle; O is the elevation angle; Y is 
the azimuth angle. 

The transformation from body coordinate system 
velocities to inertial coordinate system velocities is given 
by 

cP cO c¥SOsO@-s¥ch c¥sOchH+s¥ sO jiu 


Me 
Y| -|sY cO cWcob«sWsOes -cV só + sV s@ c% || v (3.2) 
Z -s0 ce so ca co W 


where c represents the cosine function and s represents the 
sine function. 4$, O, and Y are the vehicle Euler angles 
described above. This transformation matrix is orthogonal 


so its inverse is equal to its transpose. (Ref. 13:p. 115] 
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The transformation from body angle rates to Euler 


angle rates is given by 


$ 1 tan@sin® tan8cos9 lp 

8| =|o cos® -sin® lig (3.3) 

y O sind/cosdP cosd/cos8||r 
where the variables are as defined above. (Ref. 14:p. 12] 

b. Inputs 
The vehicle control inputs for both of the models 
are similar. Rudder angle, dive plane angle and engine rpm 
are the inputs. Angles are measured in radians. Because 
both vehicles have stern dive planes and bow dive planes 
which can operate independently, there is a separate input 
for each. The NPS AUV II also has bow and stern rudders 
which can operate independently so that model has provision 
for a separate input for each. 
C. Differences in the Models 
There are a few other differences between the SDV 

model and the NPS AUV II model. The main differences are in 
the hydrodynamic coefficients and the mass matrices of the 
two models. The hydrodynamic coefficients describe the 
effect that the vehicle velocity and angular velocity (in 
three-dimensional space) have on the hydrodynamic forces 
that act on the vehicle. The mass matrix is a convenient 
way to gather terms in the vehicle equations of motion which 


multiply linear and angular accelerations so as to 
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facilitate a state-space formulation of the model. The mass 
matrix is made up of coefficients which describe vehicle’s 
mass moment of inertia tensor, as well as the coefficients 
which describe the coupling between the linear and angular 
accelerations and the forces acting on the vehicle. The 
coefficients for both vehicles have been previously 
determined (Ref. 12](Ref. 15]. 

The propulsion models are also different. Both 
models utilize square-law thrust and drag relationships, as 
well as cross flow force and torque calculation. The SDV 
model uses a four term Simpson's Rule integration to 
calculate these forces. In fact, only two terms are 
calculated. One which corresponds to the cross flow force 
and torque in the vertical plane, NORPIT, and one which 
corresponds to the cross flow force and torque in the 
horizontal plane, LATYAW. The NPS AUV II model uses a 15 
term trapezoidal numerical integration scheme to calculate 
these cross flow forces and torques. This model also 
distinguishes between the forces and the torques in that 
four separate terms are calculated. The propulsion model 
for the AUV is deemed to be the most accurate of the two. 

3. Computational Aspects of the Equations 
To facilitate their use in the navigator, some minor 
changes have been made to both models. The changes were 


necessary because of the way the C programming language 
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handles floating point numerical operations Logic 
statements have been added to ensure that the tangent and 
Sine functions return a value of zero when passed a value of 
zero. This change makes the C programs behave more like 
their corresponding MATLAB functions. 

Another change was to take the absolute value of all 
floating point numbers prior to performing any square root 


operation. This prevents square root domain errors. 


D. MEASUREMENTS 

In addition to the vehicle model, the simulations use a 
nonlinear measurement model. The instruments which are used 
to generate the measurements are rate gyroscopes, the depth 
cell, a triad of accelerometers and the heading gyroscopes 
The depth cell reading is also used to generate a depth-rate 
estimate by simple first order difference equation. The 
measurements from the rate gyros and the depth cell are 
essentially linear functions of the vehicle state; however, 
the other measurements are related to the state nonlinearly. 

1. Depth Rate 

Depth rate is approximated as the difference between 

two successive depth measurements divided by the sampling 
interval. Analytically, however, depth rate is related to 
the vehicle’s orientation and its three-dimensional velocity 


vector. The equation which describes this relationship is 
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one of the nonlinear functions within the vehicle models, 


and is given by 


Z = -usin@+vcos@sin®+wcos®@cos ® (3.4) 


which is identical to the bottom equation of Equation 3.2. 
By taking the partial derivatives of this function with 
respect to the state, the associated portion of C matrix to 
be used with the Extended Kalman filter is generated. 
2. Accelerometers 

The measurement associated with the accelerometers 
is also a nonlinear function of the state. The 
accelerations of the vehicle fall within the dead zone of 
the accelerometers which were purchased for use in the NPS 
AUV II, and as such, these accelerometers could not be used 
in the normal sense. However, because an accelerometer 
cannot distinguish between accelerations and gravitational 
forces, the accelerometer triad would give the decomposition 
of the gravitational acceleration vector in vehicle 
coordinates. So while gravity remains constant, the 
accelerometer measurements would change depending on the 
attitude of the vehicle. The relationship between the 
accelerometer readings and the vehicle orientation is 


nonlinear, and is given by 
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av] = -RÍO (3.5) 


where R is the inverse (transpose) of the transformation 
matrix given in Equation 3.2, and g is the magnitude of the 
gravitational acceleration. 

Additionally, because these accelerometers have a 
dead zone, it is likely that there will be times when they 
do not generate a signal. This dead zone is an additional 
nonlinearity which can be taken into account in the Extended 
Kalman filter by setting to zero the row of the linearized C 
matrix that is associated with whichever accelerometer is 
operating in its dead zone. The model used for the 
accelerometer dead zone is given in Figure 3.4. 

3. Gyroscopes 

While the measurements from the rate gyroscopes are 
linear functions of the state vector, that is, they should 
be measurements of p, q, and r, it is known that the 
gyroscopes installed in the NPS AUV II generate a bias term 
that is drifting time. For this reason, the vehicle model 
has been augmented to include three more states, namely, the 
rate- gyroscope biases. Carried as states, the rate 
gyroscope measurement can then be modeled as the vector sum 
of the three angular rate vehicle states, p, q, and r, and 


their corresponding bias terms. The Extended Kalman filter 
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Figure 3.4 Model for accelerometer dead zone 


is then able to estimate the bias terms and remove their 
effect from the estimation of the vehicle state. 

In short-range problems, the heading gyroscope does 
not suffer from the bias problems that plague the rate 
gyroscopes. It has a clean signal that does not drift with 
time, and it has a magnetic flux gate sensor which helps it 
to maintain inertial alignment. 

The NPS AUV II also has a vertical gyroscope which 
measures the vehicle roll and pitch angles which are used in 
lieu of the accelerometer measurements described above. The 
measurements from this unit suffer from two problems. The 


first is quantization noise. The roll measurements are so 
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small relative to quantization levels in the analog-to- 
digital converter that the signal is lost in quantization 
noise. The second problem is with the pitch measurement, 
which is known to have a bias. As with the rate gyroscopes, 
the Extended Kalman filter for the vehicle can be augmented 
to include the pitch measurement bias which is modeled as a 
relatively constant value which is affected by a fictitious 


noise term. 


E. MATLAB MEX FILE GENERATION 

The programs developed for this thesis are written for 
MATLAB, an interactive application designed to use matrices 
as the basic computational entity. The specific version 
that is used is AT-MATLAB Version 3.5k for IBM compatible 
personal computers. 

MATLAB provides an interpretive programming environment 
which is easy to use and easy to debug in the form of Script 
files. A Script file allows a user to write MATLAB commands 
to an ASCII file. When the file is invoked, MATLAB reads 
the file and performs the operations listed therein. In 
this context, MATLAB functions as an interpreted language. 

MATLAB also has facilities for using Functions. A 
Function is like a Script file but it contains the reserved 
word, Function. A Function is different from a Script file 
in that the commands are interpreted, compiled and stored in 


RAM, ready for further uses. The advantage of using a 
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Function is that once used, the Function will run faster on 
future calls than an identical Script file because it is 
compiled. (Ref. 16:p. 2-86] 

MATLAB also allows the use of specially written FORTRAN 
and C programs to be called from within the MATLAB 
environment as though they were either MATLAB Script files 
or Functions. Files which are written using this feature 
are called MEX files. The two largest subroutines written 
for this thesis are in the form of MEX files. They were 
written in C, and then compiled, and linked with MATLAB 
Supplied libraries into executable code. This executable 
code was then operated on by a MATLAB supplied program to 
convert in to a MEX file. The only advantage to using a MEX 
file is speed of execution. A MEX file will typically run 
25 times faster than its Script or Function file 
counterpart. [Ref. l6:p. 1-47] 

The procedures for using this feature are contained in 
the MATLAB User's Guide. However, what is not explained in 
the manual is that the compilation and linking must be done 
from within the MEX subdirectory (where MATLAB is the parent 
directory). If this is not done, the linker will give 
“undefined function" errors for any Structures or Procedures 
defined within the MATLAB MEX libraries which are called by 
the C program. 

There is a significant difference between the way MATLAB 


and FORTRAN store two-dimensional arrays and the way that C 
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stores them which must be accounted for when using C 
language MEX files. Both MATLAB and FORTRAN store arrays in 
a column by column format in contiguous long-words of 
memory, whereas C uses a row by row scheme. This is not a 
difficult problem to overcome, but it forces the C 
programmer to either use transposed matrices in his code or 
to use a separate conversion routine before passing matrices 
into or out of MATLAB. The former approach is more compact 
but the latter approach makes the source code more 


understandable. 


F. PROGRAM STRUCTURE 

The programs used in this thesis are listed in Appendix 
A and Appendix B. For the SDV, the programs are SIMUL.M, 
MODEL.C, MKABMEX.C, GETMEAS.M, and MAKMEAS.M. For the NPS 
AUVII, the programs are AUVSIM.M, AUV2.C, AUV2AB.C, 
GETMEAS.M, MAKMEAS.M, MAKEK.M, and GETK.M. The names of the 
source code files for different versions of the programs are 
appended with the version number, for example, SIMUL6.M, 
MAKMEAS4 .M. 

The structure for both sets of programs is the same. 
Both SIMUL.M and AUVSIM.M are the main modules of the 
navigator. They initialize the filter parameters, the 
Simulated vehicle state, the Extended Kalman filter state, 
the control input, and the A, B and C matrices of the 


linearized model. In the case of the versions of AUVSIM.M 
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which use data recorded from the actual NPS AUV II vehicle, 
the programs read the data file as a standard MATLAB matrix 
instead of running a separate vehicle model to generate 
Simulated measurements. 

The main loop in these programs is diagrammed in Figure 
ENS First, the input 
command is generated and 
formatted. Next, the 
linearized model A, B and 


Relinearize System ; 


C matrices are j 
Calculate Kalman Gain | 
or 


recalculated around the \ Look up Kalman Gain 


last estimated state. 


Generate Measurements | 


These matrices and the Format Measurements 
error variance matrix are Predict Vehicle State 
passed to KALM.M which Generate Predicted Measurements 
payculates the next i un 
Extended Kalman filter 


gain matrix, K, using the 





formulae described in Figure 3.5 Main loop in navigator 


Chapter II. The program 

then either calls MODEL.MEX or AUV2.MEX to generate the next 
simulated vehicle state. A call is made to GETMEAS.M to 
extract the simulated gyroscope and accelerometer readings 
from the vehicle state. GETMEAS.M uses the MATLAB RAND 
function to simulate noise in the sensor readings by adding 


a normally distributed, zero-mean, pseudo-random number to 


SS 


the simulated measurement. The variance of this pseudo- 
random number is equal to the apparent variance of the 
actual sensor signal. If recorded data is used, then a 
version of GETMEAS.M is used to format the recorded data for 
use by the Extended Kalman filter and noise is not added. 

The Extended Kalman filter is implemented in the 
predictor-corrector form. Either MODEL.MEX or AUV2.MEX is 
used to predict the next vehicle state from the last 
estimate and the given inputs. The measurement equation is 
applied to this prediction to form the estimated 
measurement. This estimate and the actual measurement are 
applied to the predicted state as described in the last 
chapter to correct prediction. This corrected prediction 
becomes the estimated state for the next iteration of the 
filter: 

The sequence is somewhat different for that version of 
AUVSIM which uses the piecewise constant K matrix.  MAKEK.M 
is called outside the main loop of the program to calculate 
the steady-state K matrices for several different values of 
u and ©. Inside the loop, GETK.M is used to compare the 
current estimates of u and € to the values used to make the 
several K matrices and then return the K matrix which 


corresponds most closely to the estimated state. 
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IV. SIMULATION RESULTS 


A. GENERAL 

This chapter describes the results of the navigator 
Simulations for the SDV and the NPS AUV II. Both navigators 
were driven by simulated measurements generated by nonlinear 
models. 

The effectiveness of including a model of the bias in 
the rate gyroscopes and the use of accelerometer 
measurements to determine vehicle attitude are explored in 
this chapter. Also, the feasibility of using depth rate, as 
discussed in the Chapter III, to estimate forward speed is 
addressed, and the effect of filter parameters on the 


estimated vehicle states is discussed. 


B. SWIMMER DELIVERY VEHICLE SIMULATION 

Figures 4.1 through 4.14 show the simulated vehicle 
states and the estimated states, as determined by the 
navigator, for the SDV. For the particular simulation run 
from which this data is taken, the vehicle was given an 
initial forward speed, u, of 0.3 feet per second, a constant 
propeller speed of 650 rpm, a constant rudder command of 0.2 
radians, and a sinusoidal dive plane command. A sampling 


interval of one second has been used. Also, an initial 
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pseudo-random Gaussian error vector was added to the initial 
vehicle states to simulate an unknown initial condition. 

The remainder of this section presents the details of 
the simulated responses and attempts to explain the behavior 
of the signals. 

Figure 4.1 shows the X-Y position of the vehicle and the 
navigator. The two trajectories are not coincident 


indicating that the navigator does not generate a highly 





Figure 4.1 SDV simulation - X-Y position plot 
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accurate position estimate at all times. This is due in 
part to the lack of position error feedback which cannot be 
accomplished because position is not measured. This error 
is also due to the effects of the transients in the Extended 
Kalman filter. As shown in Figures 4.2 through 4.4, these 
transients affect the velocity estimates from which position 
is generated. 

The forward speed is estimated accurately after 
approximately 40 seconds as shown in Figure 4.2. As 


previously mentioned, the estimation error at the beginning 
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Figure 4.2 SDV Simulation - Forward velocity 
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of the run is due to filter transient effects. The 
sinusoidal variation of the speed is due to the drag induced 
by the dive plane angle and resulting porpoising maneuver. 
This effect is much more pronounced in v and w. 

Both v and w exhibit similar transient error periods. 
Figure 4.3 shows that the lateral velocity, or side slip, v, 
approaches a constant value induced by the rudder command 
and resultant turn. The convergence of the filter is faster 
for this state than for the forward speed. The dive plane 


commands also affect the side slip as evidenced by the 


seconds 





Figure 4.3 SDV Simulation - Lateral velocity 


oscillation about 0.7 feet per second which is caused by the 


rolling motion of the vehicle, shown in Figure 4.5. 
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The sinusoidal variation in w, shown in Figure 4.4, is 
much more pronounced than for either of the other two 
velocity terms because the forcing function of the dive 
command is in the same plane as w. Like forward speed, the 
filter does not converge to the correct value of w as 
ru ly as it does for lateral velocity. This is due in 


part to the large relative error between the vehicle state 
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Figure 4.4 SDV Simulation - Vertical velocity 


and the filter state not present in either of the two other 
velocity terms. This error also manifests itself in the 
pitch and pitch rate estimates. Finally, the mean value is 


not zero because the vehicle tends to roll and dive in a turn. 
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As mentioned above, the dive commands combine in the 
turn to create a varying roll effect. Figure 4.5 shows the 
vehicle’s roll rate and estimated roll rate, p. The high 
frequency oscillations in the first 30 seconds of the 
Simulation run are caused by the vehicle’s response to an 


initial roll rate at low forward speed. At low speed, the 
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Figure 4.5 SDV Simulation - Roll rate 


hydrodynamic forces on the vehicle are not sufficient to 
damp this mode of oscillation. The lower frequency 
oscillations are caused by the dive plane commands. 

Because all three angular rates are measured, the filter 


converges to the proper values more quickly than for the 
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velocity terms previously discussed. “The measurement noise 
is also evident in these three estimated states. 

Figure 4.6 shows the vehicle’s pitch rate and the 
estimated pitch rate, q. The general shape of this plot 
corresponds to that of Figure 4.4 in that the magnitude of 
the oscillation increases with vehicle forward speed and the 
mean value is not zero. The filter transient effect is very 


pronounced in the g estimate. The spike occurring at the 
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Figure 4.6 SDV Simulation - Pitch rate 


one second point is related to the initial estimation error 


in body vertical velocity, w. 
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In a similar manner, the vehicle’s yaw rate and the 
estimated yaw rate, r, reflect the estimation errors in v, 
as shown in Figure 4.7. Although the rudder command is 
constant for this run, the turn rate increases and 
approaches a steady-state value because the vehicle speed is 


increasing in the beginning of the turn and because the 
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Figure 4.7 SDV Simulation - Yaw rate 


vehicle has significant rotational inertia in the yaw 
direction. The nonlinearities in the model are evidenced by 
this plot in that the sinusoidal dive plane commands do not 
produce a strictly sinusoidal change in yaw rate. 

The scale in this figure tends to hide the measurement 


noise, but examination of the data indicates that some 
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portion of the measurement noise is present in the estimate. 
As with all the measured states, the sensitivity of the 
filter to this noise could be changed by adjusting the R 
matrix. 

Figure 4.8 shows the vehicle's depth and the estimated 
depth. As previously mentioned, the vehicle’s unusual shape 
causes it to dive in a turn. Because a constant turn is 
being simulated, the depth does not approach a steady-state 
value. Additionally, while the dive plane commands are 


Symmetrical, the dive response in the turn is asymmetrical. 
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Figure 4.8 SDV Simulation - Depth 
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The relatively large range on the y-axis of this plot does 
not allow the filter transient to be seen, nor does it allow 
the difference between the vehicle depth and the estimated 
depth to be seen; however, these effects do exist. 

The last three vehicle states to be discussed are the 
Eulerian attitude angles. As discussed in Chapter III, 
these estimates are derived from the nonlinear accelerometer 
measurement. The measurement is highly nonlinear, and 
therefore can lead to instabilities in the Extended Kalman 
filter. By making the corresponding elements of the R 
matrix relatively large, 10 (feet/second^)^, the filter is 
made more robust to this nonlinearity. 

Figure 4.9 shows the vehicle's roll angle and the 
estimated roll angle, 9$. The mean value of the roll is not 
zero because the vehicle rolls in a turn. The horizontal 
intervals in the estimated roll angle are caused by the 
accelerometer dead zone. As discussed in Chapter III, the 
row of the C matrix which corresponds to an accelerometer 
estimated to be operating in its dead zone is set to zero so 
that the Extended Kalman filter does not expect a 
measurement. The graph has discontinuities because the 
estimated accelerometer measurements and the simulated 
accelerometer measurements do not reach their dead zones 


simultaneously. 
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Figure 4.9 SDV Simulation - Euler roll angle 


The Euler elevation angle (pitch), O, estimate, shown in 
Figure 4.10, exhibits a filter transient response that 
combines the effects of both the w and q estimates; however, 
the estimate converges well and tracks the vehicle pitch 
accurately in spite of the nonlinear accelerometer 
measurement associated with this state. 

The heading angle, Y is not as sensitive to the 
aecelerometer measurement, although it is in that 
measurement equation, because it is measured directly by the 


Simulated heading gyroscope. Figure 4.11 shows that the 
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Figure 4.10 SDV Simulation - Euler elevation angle 


estimate and actual state are virtually indistinguishable. 
As with the depth plot, the range of the vertical scale 
prevents differences between the vehicle state and the 
estimated state from being observed. Because the heading 
gyroscope is reliable, the elements of R matrix associated 
with Y were made small so the filter would track the 
measurement closely. 

Finally, Figures 4.12, 4.13, and 4.14 show the estimated 
bias terms for each of the three rate gyroscopes. These 
bias terms were simulated by adding a constant term to each 
of the rotational rate states before passing them to the 


Extended Kalman filter as measurements. 
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Figure 4.13 SDV Simulation - Estimated pitch rate gyroscope 
bias 
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Figure 4.14 SDV Simulation - Estimated yaw rate gyroscope bias 


In all cases, the filter is able to accurately estimate 
the simulated gyroscope bias and thus remove its effect from 


the measurements. 
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C. NPS AUV II SIMULATION 
1. Typical Maneuver 

Figures 4.15 through 4.28 show the simulated and 
estimated states of the NPS AUV II performing a maneuver 
Similar to that of the SDV described in Section B. As in 
the previous section, the vehicle was given a low initial 
forward speed, u, of 0.3 feet/second, a constant propeller 
speed of 550 rpm, a constant bow and stern rudder command of 
0.2 radian and -0.2 radian respectively, a sinusoidal dive 
plane command and an initial error between the filter states 
and the vehicle states was introduced. A sampling interval 
of 0.2 seconds has been used. 

The results of this simulation are similar to those 
depicted for the SDV in the previous section. Because the 
two vehicles are geometrically similar, they share the 
common characteristic of diving in a turn. However, because 
the NPS AUV II is a much smaller vehicle, its dynamics are 
much faster and the Extended Kalman filter responds 
differently. 

The noise rejection performance of this Extended 
Kalman filter is poorer than in the SDV simulation, although 
the noise covariance matrices used were the same for both, 
and the absolute magnitude of the additive pseudo-random 


noise was identical for both. The difference can be 
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attributed to the different dynamic behavior exhibited by 
the vehicles. 

There is one difference in the measurement noise 
covariance matrix, R, between the two simulations. The 
elements associated with the accelerometer sensitive to 
pitch have been increased by a factor of ten to 100 
(feet/sec^)^. If this were not done, the filter would be 
not be stable. 

Although Figure 4.15 seems to imply that the 
navigator for the NPS AUV II is not as accurate as the 


navigator for the SDV, the scale is misleading. As with the 
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Figure 4.15 NPS AUV II Simulation - X-Y position plot 
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SDV, the position error shown here is due to the velocity 
errors in the state estimates which are caused by the 
transient response of the filter. The transient is induced 
in large part by the initial random vehicle state. Although 
other transient errors exist, it is the velocity error that 
has the most impact on the position estimate. Other 
Simulation runs have produced more accurate position 
estimates, but this run is included because the random 
initial estimation error seems to have caused the filter to 
perform at its worst. 

The random initial error does not appear to have 
affected the forward speed estimate, as shown in Figure 
4.16. This figure also shows that the forward speed of the 
vehicle is not strictly first order as evidenced by the 
slight overshoot. This overshoot is attributed to the 
relative rotation of the three-dimensional vehicle velocity 
vector with respect to the vehicle coordinate system that 
occurs when the vehicle turns. Before the vehicle yaws into 
a turn all the velocity is along its longitudinal axis. As 
it begins to turn this velocity is distributed into the 
other velocity component directions, as shown in Figure 4.17 
and Figure 4.18, and the forward speed drops. These effects 
would not be apparent in a dead-reckoning navigator, but 
Wey are predicted and taken into account by the Extended 
Kalman filter. The SDV does not exhibit this behavior 


because of its different size and kinetic characteristics. 
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Figure 4.16 NPS AUV II Simulation - Forward velocity 


The sinusoidal variation in u present in the SDV 
simulation is not present in the NPS AUV II simulation 
because the vehicles have different hydrodynamic 
characteristics. 

The lateral velocity, v, shown in Figure 4.17, 
approaches a constant value as in the SDV simulation; 
however, unlike the SDV, the roll effect is not pronounced 
so there is little variation in the lateral velocity once it 
reaches steady state. The estimation error seen in the 
beginning of this run is induced by the random disturbance 


added to the initial vehicle state. 
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Figure 4.17 NPS AUV II Simulation - Lateral velocity 


The initial error in w does not affect the position 
estimate as greatly as u and v do. However, the large 
initial estimation error and transient period in w is 
aated to both vehicle's and the filter's transient 
response in pitch rate and pitch angle. 

The sinusoidal variation inw, shown in Figure 4.18, 
is caused by the dive plane commands which induce a change 
in this component of the vehicle velocity as described in 
Section B. 

The measurement noise rejection performance of the 
NPS AUV II navigator is exhibited by Figures 4.19, 4.20, and 


4.21, which show the angular rate estimates. As with the 
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Figure 4.18 NPS AUV II Simulation - Vertical velocity 


SDV, simulated measurements from the rate gyroscopes are 
used in the estimation of these states. Compared with the 
corresponding figures from Section B, these estimates are 
much more affected by measurement noise. 

Although the roll rates shown in Figure 4.19), 
exhibits the same high frequency oscillation as in the SDV, 
the magnitude of the roll rate induced by the dive Comings 
is not as large. Moreover, the estimated roll rate does not 
converge to the actual value ás quickly as for the SDV 
While this is partially caused by the initial estimation 
error, it is more closely related to the different dynamic 


behavior of the two vehicle models. 
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Figure 4.19 NPS AUV II Simulation - Roll rate 


As with the roll rate, the pitch rate estimate, 
shown in Figure 4.20, exhibits high frequency measurement 
noise. The filter's transient response, evident in this 
figure, also shows that the initial error in pitch rate is 
related to both the initial error in w and in pitch, O, as 
previously mentioned. 

The transient response of the vehicle is also 
evident in this figure. The behavior through the first 20 
seconds of the simulation is due to the random initial 
condition of the vehicle causing it to follow a complicated 
trajectory which has the result of making the filter less 


effective in estimating the states. 
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Figure 4.20 NPS AUV II Simulation - Pitch rate 


As with the SDV, the NPS AUV II rolls and dives 
during the turn; therefore, the average roll rate and pitch 
rate shown in the two previous figures are not zero. 

The yaw rate of the NPS AUV II, shown in Figure 
4.21, differs from the yaw rate of the SDV shown in Section 
B in that the coupling between the dive plane commands and 
the yaw rate is not evidenced in the response of the AUV II. 
The oscillation evident in Figure 4.7 is not present in 
Figure 4.21. This is due to the lower cross-coupling 
effects between roll and pitch discussed above. 

After the initial negative spike, the yaw rate 


estimate quickly converges to the actual yaw rate and tracks 
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Figure 4.21 NPS AUV II Simulation - Yaw rate 


it through its transient period, although it does exhibit 
some measurement noise corruption. 

While both the SDV and the NPS AUV II dive in the 
turn, Figure 4.22 shows that the smaller vehicle does not 
exhibit this characteristic to as great a degree as the SDV. 
This fact is related to the smaller roll angle experienced 
Pyeene venicle during the turn shown in Figure 4.23 and 
evidenced in the roll rate and lateral velocity. 

For the depth estimate, the Extended Kalman filter 
does not have a significant transient period and the 


estimated state converges more quickly to the actual value 
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Figure 4.22 NPS AUV II Simulation - Depth 


than the other states do, even in the presence of 
measurement noise and the initial estimation error. 

Figures 4.23 through 4.25 show the last three state 
estimates, the Euler angles. In general, these show a more 
Significant transient period than for any of the other 
estimates. Moreover, the Extended Kalman filter does not 
reject the noise for roll or pitch as well as it does for 
the rate gyroscope measurements. 

The noise evident in both the roll and pitch 
estimates is caused partially by the noise added to the 
Simulated accelerometer measurements. It is also caused by 


the nonlinear nature of these measurements including both 
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Figure 4.23 NPS AUV II Simulation - Euler roll angle 


the trigonometric nonlinearities in the coordinate 
transformation equation and the accelerometer dead zone 
nonlinearity. 

The heading estimate, shown in Figure 4.25, does not 
appear to have the noise corruption evident in the other two 
Euler angle estimates. This is because the heading is 
measured directly by the heading gyroscope. While the scale 
hides any noise present, the heading gyroscope does not 
generate a noisy signal and so little noise (standard 


deviation of 0.001 radians) was added in the simulation. 
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Figure 4.24 NPS AUV II Simulation - Euler elevation angle 


Finally, Figures 4.26, 4.27, and 4.28 show the 
Extended Kalman filter’s estimate of the rate gyroscope 
biases. These bias terms have been simulated as described 
in Section B and were given the same values as in the SDV 
Simulation run for more consistent comparison. As with the 
SDV navigator, the NPS AUV II navigator is able to estimate 


this simple bias model. 
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Figure 4.25 NPS AUV II Simulation - Euler azimuth angle 





Figure 4.26 NPS AUV II Simulation - Estimated roll rate 
gyroscope bias 
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Figure 4.27 NPS AUV II Simulation - Estimated pitch rate 
Gyroscope bias 





Figure 4.28 NPS AUV II Simulation - Estimated yaw rate 
gyroscope bias 


2. Estimation of Forward Speed from Depth Rate 
Figures 4.29, 4.30 and 4.31 show the results of 


speed estimation correction using depth-rate and pitch 
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information. For this run the input has been changed froma 
steady turn to a straight line and the dive plane command 
has been changed to a square wave. More importantly, 
however, a 10 percent difference between the simulated 
vehicle’s propeller speed and the Extended Kalman filter 
propeller speed input has been deliberately introduced. The 
filter has been given a slower propeller speed and thus it 


tends to generate a lower forward speed estimate. 
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Figure 4.29 NPS AUV II Simulation - Forward velocity with 
propeller speed error 


By changing the term in the Q matrix associated with 
the expected disturbance on the forward speed to 20 
feet^/second^ from 0.05 feet?^/second?^, the estimate of 


forward speed has been made more sensitive to the 
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correlation between depth rate and velocity. As shown by 
comparing Figures 4.29, 4.30 and 4.31, the estimate ofm 
improves when the absolute magnitude of the pitch angle is 
high and when absolute magnitude of the depth rate is high. 
It is interesting to note that iñ this simulation mie 
estimated speed actually increases when the dive planes are 
at their maximum deflection, a condition which induces drag 
and which would normally cause the vehicle speed and the 


estimated speed to drop. 


seconds 





Figure 4.30 NPS AUV II Simulation - Euler pitch angle with 
propeller speed error 


Because the Extended Kalman filter does not expect a 
steady-state error in u, the estimate oscillates. The 
frequency of the oscillation is at twice the frequency of 
the pitch and depth rate because the speed estimation is 
affected by the magnitude of these signals rather than their 


sign. 
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Figure 4.31 NPS AUV II Simulation - Depth rate with propeller 
Speed error 


To remove the variation in the forward speed 
estimate and to force the estimate to more closely track the 
actual vehicle speed in the presence of the induced error, 
i.e. when the forward speed prediction does not correlate 
with the estimated pitch and depth rate, the predictor was 
changed to include a constant propeller-speed bias term much 
like the rate gyroscope bias terms. This was done by 
appending the term from the linearized B matrix which 
relates propeller speed to forward speed to the linearized A 
matrix. The plant noise covariance matrix, Q, and the 
initial filter error covariance matrix, P, were modified to 
include this bias. The element of Q associated with forward 
speed was reduced to cause the filter to compensate for the 
prediction inconsistency by correcting the value of the 


propeller bias. 
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Some results of this modification are shown in 
Figures 4.32 and 4.33. As expected, the variation 11 me 


forward speed estimate is reduced; however, a steady-state 
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Figure 4.32 NPS AUV II Simulation - Forward velocity with 
propeller speed bias 


error still exists between the estimated and the actual 
speeds. This error is a function the linearized model. 

The actual propeller error introduced in this 
imulation was 55 rpm; however, as shown in Figure 4.33, the 
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rror at approximately 54 rpm. 
Additionally, the filter was given an initial condition for 
the bias of 50 rpm. While this is artificial, it did allow 


the filter to reach an apparent steadv-state value in a 
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Figure 4.33 NPS AUV II Simulation - Propeller speed bias 


short amount of time. If the bias were initialized at zero, 
then the filter would not have reached steady state in the 
Simulated run time. It was found that the value of the bias 
State changed more slowly as the eigenvalues of the error 
covariance matrix became slower. 

Because there is a steady-state error and because 
the length of time required by the filter to reach a steady- 
state value is approximately equal to the run time of a 
typical pool mission for the NPS AUV II, this approach to 
correcting a speed-estimation error is not used to filter 


the recorded experimental data. 
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V. EXPERIMENTAL RESULTS 


A. GENERAL 

This chapter presents the results of using actual data 
recorded from the instruments on board the NPS AUV II as 
input to the navigator rather than simulated measurements as 
in Chapter IV. The data was taken during a 100 second test- 
and-evaluation mission conducted in the Naval Postgraduate 
School’s swimming pool on 26 August 1991. The data was 
recorded during the second mission run by the vehicle that 
day during which the vehicle made a single U-turn while 
porpoising at a depth of approximately two feet. A 
piecewise constant Extended Kalman filter gain has been used 
with a table look-up scheme to process this data. The 
navigator’s output and the differences between the actual 


and expected results are discussed. 


B. IMPLEMENTATION 

The data recorded from the vehicle's instruments include 
a time record, a depth cell measurement, a paddle wheel 
speed log measurement, three rate gyroscope measurements, 
two measurements from a vertical gyroscope (roll and pitch), 
dive plane and rudder commands, and left and right propeller 
shaft speeds. There were no accelerometers in the vehicle 


at the time the data was recorded. The instruments on board 
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the NPS AUV II have analog output which is converted to 
sag ical fornmebyethe on board computer. 

The data sampling interval was 0.1 second. Although the 
instruments were sampled at this rate, the navigator uses 
data at every other sample time, or at 0.2 second intervals. 
This reduces the navigator’s computational requirements 
without sacrificing performance. 

To further reduce the computational burden, the Extended 
Kalman filter has been implemented with piecewise constant 
gain matrices instead of the time varying matrices used in 
the simulations described in Chapter IV. Using this method 
obviates the calculation of the gain matrix on-line. 

Because the gain matrix is not calculated on-line, the 
relinearization and discretization of the vehicle model, 
required if the fully time-varying Extended Kalman filter 
were used, are not needed. 

To use piecewise constant K matrices, several steady- 
state gain matrices were calculated for a given set of 
estimated states. Several values of forward speed, u, and 
Euler pitch angle, O, were selected. For each combination, 
the corresponding steady-state K matrix was calculated and 
stored. The built-in MATLAB function DLQE, (discrete linear 
quadratic estimator) was used to calculate the gain 
matrices. The respective values of both states used to 


generate the results presented this chapter were 
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u e 15 2.0 treet, see 
(5.1) 
Ə e {(-0.05 -0.025 0.0 0.025 0.05} radian. 


While other values have been used, these give satisfactory 
results because the measurements are sensitive to estimated 
pitch and these pitch intervals are small making the gain 
approximation better. 

The net result of using a 0.2 second discrete time 
interval and using the piecewise constant K matrix is that 
the 100 second mission can be processed in approximately 40 
seconds of processor time (MS-DOS PC with a 33 MHz Intel 
80386 processor). If the entire algorithm were written in 
C, instead of MATLAB and C, it is expected that this time 


would be reduced significantly. 


C. NAVIGATOR OUTPUT 

Figures 5.1 through 5.16 show the estimated states 
generated by the navigator using the recorded data. For 
those states which correspond to an instrument output, the 
figure displays both the recorded measurement and the 
estimated state. 

The remainder of this section describes the details of 
the Extended Kalman filter navigator responses to the 
recorded data. The pertinent characteristics are explained. 


This follows closely the format of the previous chapter. 
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Figure 5.1 compares the position estimate produced by 
the navigator with a dead-reckoning estimate. The dead 


reckoning plot was generated using the recorded vehicle 





Figure 5.1 - NPS AUV II - X-Y position plot 


speed and heading gyroscope measurements. No accurate 
record of the actual position of the vehicle during this 
mission is available. The borders of the graph correspond 
approximately to the dimensions of the ae The initial 
position of the vehicle was approximately 20 feet from the 
short wall and 15 feet from the long wall. There is a 
Significant difference between the dead reckoning plot and 


the navigator output which is attributed to the side slip, 
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v, experienced by the vehicle in the turn, which is not 
taken into account in dead reckoning. 

The estimated forward speed corresponds well to the 
speed log measurement as shown in Figure 5.2. The 
variations in the speed are caused by both the vehicle's 
speed controller which uses unfiltered speed log 
measurements in feedback and the dive plane commands, which 


induce drag. During the vehicle’s 180 degree turn, which 
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Figure 5.2 NPS AUV II - Forward velocity 


Started at approximately the 40 second mark, a significant 
difference between the measured speed and the estimated 
speed develops. The difference is due in part to errors in 


the model and in part to the side slip velocity, shown in 
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Figure 5.3. Side slip causes the paddle wheel to generate 
erroneous speed measurements. 

The estimated side slip velocity, v, shown in Figure 
5.3, approaches a minimum value of approximately -0.5 
feet/second during the turn. There is no instrument on the 
vehicle to measure this quantity so there is no measurement 
for comparison. The oscillations present prior to the turn 


are due to the low initial vehicle speed, which makes the 
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Figure 5.3 NPS AUV II - Estimated lateral velocity 


vehicle itself underdamped in yaw, and the vehicle’s heading 
controller which is also underdamped. During the turn, the 
estimated side slip approaches -0.5 feet/second and has not 


returned to a zero mean value by the end of the run. This 


nts 


lattér fact rësults in the ångled trajectory in CMe X-Y 
pláné evidènt in the- return-leg pomt on of Eigur sS 1. 
While the estimated heading is very close to the measured 
heading used in the dead reckoning plot, the lateral 
velocity gives the vehicle an oblique path. 

The body coordinate vertical velocity, w, does not have 


a significant effect on the position estimate because the 


pitch angles are relatively low; however, this state does 


seconds 





Figure 5.4 NPS AUV II - Estimated vertical velocity 


affect the depth-rate measurement. As with side slip, there 
is no instrument to measure w. The relatively large spike 
occurring in the first few seconds is due to both the 


vehicle and the filter transient effetts. 
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Comparison of the angular rate estimates shown in 
Proures 5.5"Uhrough 5.7 with those of the simulations 
indicates that the filter is not as sensitive to measurement 
noise as was indicated in simulation. This is largely 
because the data used in this study is relatively free of 
the noise present in other data sets. 

As in the simulations, Figure 5.5 shows that the 
estimated and the measured roll rate, p, exhibit a high 


frequency oscillation when the vehicle is operating at low 
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Figure 5.5 NPS AUV II - Roll rate 


speed. As with the side slip, the vehicle’s roll mode is 
underdamped at low speed but becomes more damped as the 


hydrodynamic forces on the vehicle increase with speed. The 
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gyroscope measurement exhibits quantization noise to a 
significant degree. The obvious bias in the measurement is 
due largely to this effect. Additionally, the measurement 
gives no evidence of roll during the turn, although the 
navigator does estimate a rolling effect between 
approximately 45 and 65 seconds. This could be because the 
model is inaccurate or because the quantization noise is 
hiding the effect. Comparison with the roll angle 
measurement indicates that it is the latter. 

Figure 5.6 shows the estimated and the measured pitch 
rate, q. As with the w estimate, both the vehicle and the 
filter transient effects produce the spike in the beginning 
of the plot. For all but the peak values, the navigator 
estimate corresponds closely to the measurement. The 
mismatch at the maximum values indicates that the model is 
in error. If it were just a bias in the measurement then 
the estimate would be low through the entire pitching 
motion: 

The estimated yaw rate differs from the other angulas 
rate estimates in that there is a period of time when the 
estimate differs significantly from the measurement. Figure 
5.7 shows that this error occurs during the turn. Althouem 
quantization noise is evident in this figure, it is not the 
cause of the error as is hypothesized for the roll rates 
Figure 5.5. The measured yaw rate reaches a maximum value 


in the turn while the estimate continues to increase. This 
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Figure 5.6 NPS AUV II - Pitch rate 


effect can be attributed to physical phenomena occurring in 

the vehicle which is not included in the vehicle model, such 
as rudder stall. Because it uses a constant bias model for 

the rate gyroscope measurements, the navigator tends to use 

the bias to account for differences in the expected behavior 
and the measurements so the estimated bias varies more than 

was expected. 

Figure 5.8 shows the estimated and the measured depth. 
There is excellent correlation between the measurement and 
the estimate. Also, because the depth signal is relatively 
free of noise, differentiating it to generate a depth-rate 


Signal is entirely feasible. 
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Figure 5.7 NPS AUV II - Yaw rate 


The Eulerian attitude angles are the last vehicle states 
estimated by the filter. As previously mentioned, only the 
pitch angle measurement and the heading measurement are used 
in the navigator. 

The roll measurement shows a significant, relatively 
constant bias as well as quantization noise. A comparison 
of the estimated and the measured roll angle, € in Figure 
5.9 shows that the dynamics of the roll measurement do not 
appear to differ greatly from the estimated roll. The roll 
measurement (excluding the bias) corresponds to the 
estimated roll, especially in the turn. The absence of a 


corresponding roll rate measurement, from Figure 5.5, 


78 


50 


seconds 





Figure 5.8 NPS AUV II - Depth 


indicates that the roll motion experienced by the vehicle 
was not detected by the roll rate gyroscope and that 
measurement is in error. 

Unlike the simulation studies, the estimated pitch 
converges quickly to the measured pitch, ©. However, Figure 
5.10 shows that there is a systematic estimation error in 
the pitch estimate. As opposed to the pitch rate 
measurement and estimate, the pitch angle estimate appears 
to underestimate the pitch throughout the cycle as though a 
bias error were present. As discussed above, this might be 
due to an error in the vehicle model or in the gyroscope 


model. 
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Figure 5.9 NPS AUV II - Euler roll angle 


Figure 5.11 shows the estimated and the measured 
heading, Y. As previously mentioned, the heading gyroscope 
measurement is very clean and the Extended Kalman filter 
relies greatly on the accuracy of this measurement for 
heading estimation. While the scale of this figure prevents 
the heading prediction error from being seen, the mean 
Square error is approximately of 0.015 radian. 

Additionally, the yaw rate error shown in Figure 5.7 is not 
evidenced in the heading estimate indicating that that error 
is insignificant for navigating a short-range mission. 

Figures 5.12 through 5.14 show the three estimated rate 


gyroscope bias terms. As previously mentioned, the Extended 
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Figure 5.10 NPS AUV II - Euler elevation angle 


Kalman filter tends to compensate for prediction 
inconsistencies by changing the bias terms. This is most 
apparent in Figure 5.15 which has a negative ramp-shaped 
section corresponding to the wedge shaped difference between 
the yaw rate measurement and estimate. It is also evident 
in the middle section of the roll rate bias estimate which 
corresponds to the vehicle’s turn. Although none of these 
bias estimates is as invariant as in the simulation runs, 
they all appear to approach some non-zero mean value when 


the vehicle is running ina straight path. 


81 


Recorded data — |.: 
Navigator 


50 


seconds 


— 
ui 
= 

qa 

= 
o 
A 





Figure 5.12 NPS AUV II - Estimated roll rate gyroscope bias 
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Figure 5.13 NPS AUV II - Estimated pitch rate gyroscope bias 
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Figure 5.14 NPS AUV II - Estimated yaw rate gyroscope bias 


In using the pitch gyroscope measurement, the filter was 
augmented to include a bias term for pitch. This is shown 
in Figure 5.15. As with the other bias terms, the estimate 
is not constant, but it does appear to evolve around some 


non-zero mean value. The variation in this bias is related 
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Figure 5.15 NPS AUV II - Estimated pitch gyroscope bias 


to the time varying pitch angle prediction error made 
evident by the vehicle's pərpelsig 

The final measurement used by the filter is depth 
which is derived from the depth cell data by a simple 
difference equation. Figure 5.16 shows the estimated 


the derived depth-rate measurement. 


D. ALTERNATE SPEED ESTIMATION 

To show that depth-rate information could be used 
generate speed estimates without knowledge of vehicle 
dynamics, the depth cell data and the pitch gyroscope 


have been processed by a simple second-order Extended 


rate 


and 


to 


data 


Kalman 


filter which does not require a dynamic model of the AUV. 


For this filter, both the speed and the pitch are modeled as 


smoothly varying signals. The measurements used are pitch 
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Figure 5.16 NPS AUV II - Depth rate 


and depth rate. The measurement equation for depth rate is 
therefore nonlinear as explained in Chapter III. The system 


equations for this estimator are given by 








Ü Z 0 0 op (5.2) 
e 0 0Jl8] Iv, 

x -p-| x | + | (5.3) 
Z -u sin® u, 








where v and p are white Gaussian noise signals. The 


linearized C matrix used by the Extended Kalman filter is 


therefore given by 
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0 1 
C = x (5.4) 
-sin@ -à cos0 


With this system of equations, @ must be non-zero in order 
for the state to be observable. 

With simulated data, these equations give perfect speed 
and pitch estimates even with a sinusoidal pitch measurement 
in spite of the linear approximation and the white noise 
assumption. A wide range of values could be used in Q and R 
without making the filter unstable. However, using actual 
data, in which other effects, including that of w, the body 
vertical velocity, are not taken into account, gives 
approximate u estimates while providing accurate © 
estimates. This is shown in Figures 5.17 and 5.18. 
Additionally, depth rate is not estimated as accurately as 
pitch as seen in Figure 5.19. 

The stability of the estimator is very sensitive to the 
values chosen for Q and R. These results were obtained with 


values of 


Ul ~O 
: (5.5) 
n 


and 
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Figure 5.17 NPS AUV II - Forward velocity using alternate 
estimation model 


& [0.001 0 | (5.6) 
0 0.01 


Other values yield a stable filter, but these values give 
the best compromise between accuracy of the three quantities 
estimated. 

If Q(2,2) were made smaller, even if the other values 
were made larger to compensate, then the speed estimate 
would become unstable and the pitch estimate would not track 
the measured pitch; however, the depth-rate estimate would 


track even the noise in the depth-rate measurement. 
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Figure 5.18 NPS AUV II - Pitch angle using alternate forward 
velocity estimation model 


Although this second-order filter does give reasonable 
estimates, and is much simpler than the full navigator, its 
estimates do not have the accuracy that can be obtained by 
using information about the vehicle dynamics. However, if 
such information is not available, then this approach could 
be used to estimate speed without accelerometers or rate 


gyroscopes. 
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Figure 5.19 NPS AUV II - Depth rate using alternate forward 
velocity estimation model 
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VI. SUMMARY, CONCLUSIONS, AND RECOMMENDATIONS 


A. SUMMARY 
This thesis presents a study of model-based navigators 
for small autonomous underwater vehicles. The approach 
taken in the design and testing of the navigators included: 
l. The development of linearized models for the SDV and 
the NPS AUV II based on nonlinear models which were 


already available. 


2. The programming in C and MATLAB of both the nonlinear 
and linearized models for both vehicles. 


3. The development and programming of nonlinear and 
linearized measurement equations for using 
accelerometers as attitude sensors. 

4. The development and programming of nonlinear and 
linearized measurement equations for using depth rate 
to estimate forward speed. 

5. Simulation studies for both vehicles using additive 
white Gaussian noise, an accelerometer dead zone model, 
and a gyroscope bias model. 


6. Experimental studies with the NPS AUV II using recorded 
instrument data. 


B. CONCLUSIONS 

In this study, a navigator which uses knowledge of the 
vehicle dynamics is developed. In particular, speed 
estimation is obtained by combining the depth rate with 


inertial measurements. 
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The following conclusions can be drawn from the results 
Gi this study: 
1. Position, attitude, velocity and angle rate estimation 
for both vehicles is possible. 


2. Velocity and attitude estimation are possible through 
the use of nonlinear measurement equations. 


3. Simulated instrument readings can be filtered and 
Simulated gyroscope bias errors can be accurately 
predicted using the navigator. 


4. Actual data can be filtered with simple instrument 
noise and error models. 


5. A piecewise constant Extended Kalman gain is adequate 
for the NPS AUV II navigator. 


6. The algorithm can be implemented in real time with a 


with a sampling rate of 5 Hz without overtaxing the 
processor. 


C. RECOMMENDATIONS 
Although manufacturers indicate that a small, accurate, 

laser-gyroscope-equipped, inertial measurement unit will be 
available in the near future, the algorithm explored in this 
thesis could be used in the interim for the NPS AUV II 
Guring pool missions. It is therefore recommended that the 
following be accomplished to facilitate implementation of 
the navigator: 

1. Convert the navigator’s main loop and other MATLAB 


functions to C and compile the entire program for use 
in the current NPS AUV II computer. 


op: 


2. Accurately record the actual position of the vehicle 
the pool for comparison with the navigator’s position 
estimates, and update the hydrodynamic coefficients and 
filter parameters as required. 


3. Use recorded gyroscope data to identify a better low- 
order model of the gyroscope measurement. 


4. Use recorded data from each measurement source to 
identify a better low-order noise model. 


If these can be accomplished then a study should be made 
to integrate differential Global Positioning System (GPS) 
data with the navigator, or to use differential GPS to reset 
the position estimate of the navigator periodically. 

Finally, it is recommended that the current research in 
expert-system sonar processing be integrated with the 
navigator. As with GPS, the sonar could be used either to 
aid the navigator in real time, or to reset the position 
estimates periodically, depending on the sonar’s accuracy 
and processing requirements. The sonar information could 
also be used to correct any unknown initial errors in the 
heading gyroscope which are not taken into account by the 


navigator. 
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APPENDIX A 


Program Listing for SDV Simulation 
% % % % % % $ % $ % $ $ % % % $ $ % $ % $ % % $ % $ $ % % $ $ % % % $ $ $ % $ % % % % $ $ % 
File SIMUL9.M 
This MATLAB Script file performs the SDV 
navigator simulation. It initializes the 
variables and contains the main loop for the 


navigator. 
Velocities are in ft/sec; angles in radians 


GETMEAS.M, MAKEMEAS.M 
Modified 11 Oct 91 
Ver.9 No speed log measurement 
Previous versions used a different accelerometer 


measurement concept and did not model 


$ 
$ 
% 
% 
% 
% 
% 
% 
% 
% Calls MKABMEX.MEX, C2D.M, KALM.M, MODEL.MEX 
% 
% 
% 
% 
% 
% 
% 
% gyroscope errors 

% 

% 


% % % % % % % % % % % % % % $ $ % % % % % % % $ $ % % % % % $ $ $ $ % % % % % % % % % % % 


% Set initlal state 


Z = 20; 

rpm = 500; 

u0 = 0.30; 
wO = 0.0; 
pitchrate = 0.0; 
pitch = 0.0; 
heading = pi/8; 
phiO - 0.0; 
theO = pitch; 
psio0 = heading; 


initial state = 
[u0;0;w0;0;pitchrate;0;0;0;2;0;pitch; heading]; 


$ Set initial inputs 
input0 = [0;0;0;rpm]; 


% Set sample interval and run time 


dt = 1.0; 
T£=100; 
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% Set up plant and measurement noise covariance 

% matrices 

% (prepare to leave out X and Y terms from Kalman filter) 
Q-zeros(13); 

Q(1:10,/1: 10) — 0 D D eve 0); 

OQ(1,1) a . 1; 
(dll So 


0,005*eve( 3); 


R=.05*eyet( 9); 
R(6,6) = 10; 


% Set up initial estimate covariance matrices for 13 states 
$ Uu,V,wWp,Q,Y,24phi, theta,ps1 plus 

% 3 rate gyro bias states 

Pold = eye(13); 


Pold(8,8) = .1; Pold(9,9) = .1; Pold(10,10) = .1; 
$ Initialize state vectors for simulation 

$ simulate unknown vehicle state with random noise in IC 
$ state - simulated vehicle 

$ navig = Extended Kalman filter navigator 
rand('normal') 

quc i2 2 

kmax -Tf/dt; 

state = zeros(12,kmax); 

State(:,l) = initial state F 0-03 eand PNE 
navig = zeros(15,kmax); 


navig(:,1) = [initial race, 0 0 0] 


% shell for measurement matrix C: 








C=[0 0 0-1 0 007010 0 110 0 ps p 
00 0-0 1'0- 0 0 0 0 071 Orme | =|q;+ biases, 
0.70.2070 -0 215001:9/201:309000 31 R. r 
0- 0 000 0/1 0 00 0 070 T3 depth 
0 070 0 070707020 070200 Ey depth dot 
0 070 0 070 0 0 0 00 0 0; P 
0 0 0 0 0 0 010. 0110000 Oras | will be =-|R]|g|] 
00 0 0 0 0:00 060 00 O R: 
0 0 0 0 070 0"0 0 19050 One heading = Ps1 


$ disturbance input matrix (different from Gam) 
Gam2=sign(Q); 


meas(:,1)=zeros(9,1); 

for (1=1:kmax) 
inputm=(0.2;0.20*cos(2*p1*1*dt/20) 0 mm], 
rnputv-[0.27;0720*cos(2*8ps so *dt/209 O rpmj]; 
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$ assign states required in C matrix relinearization 


uh - newjig(1,i);ewh semnaweg.(3yà); 
phin snaymeo i) 
theh = navig(11,1); 
polh =" nevig(i2,1); 


$ Recalculate C, Phi and Gam matrices around estimate of 
% states. For Zdot use abbreviated form of analytic 
$ equation 


€(5,1) = -—sin(theh); 

(5,35) = eCosiGgthenh); 

C(5,9) = -uh*cos(theh) - wh*sin(theh); 
(6,9) - -gecos(theh); 

p 7789) = =g*cos(phih)*cos(theh); 
C(7,9) = -g*(-Sin(theh)*sin(phih) ); 
C(8,8) = -g*(-sin(phih)*cos(theh)); 
C(8,9) = -g*(-sin(theh)*cos(phih)); 


% in case accel is in dead zone 
if (abs(meas(6,1))<.4) 

C(O, :)—m7er0s(C(6,:)); 

end; 


€ Setup the full 12 state re-linearized model 
[a,b]2mkabmex(navig(1:12,i),inputm); 


$ delete from 'a' and 'b' elements having to do with 
$ X and Y to improve stablity of Extended Kalman filter 
pe “Pat u5, 16) , d(Bub5L9 T2); 
gu E 7. 155573 qo 1279 12)]; 
Bo = (BG, : ); 
bague A 
% add gyro bias terms as constants 


a = [a zeros( 10,3); 
Zenoso 3-13) 
b = [b;zeros(3,4)]; 


% Dicretize the system 
(Phi,Gam]sc2d(a,b,dt); 


€ Calculate Kalman gain 
[K,Pnew] - kalm(Phi,Gaàm2,C,Pold,Q,R); 
Pold-Pnew; 


% Calculated next simulated vehicle state 
state(:,1+1) = model(state(:,1) ,inputv,dt); 
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$ Generate simulated instrument measurements 
[rategyro,Z,Zdot,accel,hdg]=getmeas3(state(:,i+1),2Z,dt); 
meas(:,i-1) = [rategyro;Z;Zdot;accel;hdg]; 


$ Calculate next navigator prediction 
navig(1:12,1i+1) = model(navig(1:12; 1) inputcm rs 
navig(13:15,i+1) = navig(13:15,i); 


% Generate predicted measurements 
modmeas - makmeas2(navig(:,i-*l),navig(:,i),dt); 


$ Calculate correction using Kalman gain 
esterr(:,i) = K*(meas(:,1i+1)-modmeas) ; 


% Correct prediction. Zeros are to account for X and Y 

% not being included in the Extended Kalman filter 

nay3q(:,441)— € 
navig(:,it+l)+[esterrz (136 ,1)7@; 0; eseerr (7:13 NE 


end; 
$ Generate graphical output 
t=0:dt pz. 


Idel sdvxy.met 
plot(state(?7;:),së@ate(8,:),navig(7)/:y y navig (B A: )); gr To 
xlabel(’X - feet’);ylabel(’Y - feet’);pause 

meta sdvxy 


!del sdvu.met 
plot(t,state(1,:),t,navigt(l, 7) arad 
xlabel(’seconds’);ylabel(’feet/sec’);meta sdvu; pause 


!del sdvv.met 
plot(t,state(2,:),t, navig(272)); eid 
xlabel('seconds');ylabel('feet/sec');meta sdvv;pause 


Idel sdvw.met 
plot(t,;state(3,7:),t, navig@3,:. ), 951d 
xlabel(’seconds’);ylabel(’feet/sec’);meta sdvw; pause 


idel sdvp.met 

plot(t,state(4,:),E,navia(1 2er Ta 
xlabel('seconds');ylabel('radians/sec');meta sdvp;pause 
idel sdvq.met 

plot(t,;sSstate(5,:), t; navig(5, J) grid 
xlabel('seconds');ylabel('radians/sec');meta sdvqg;pause 


!del sdvr.met 
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Bilge i, statere, s ©,navig(6,: ));qrid 
xlabel('seconds');ylabel('radians/sec');meta sdvr;pause 


!del sdvz.met 
or (t state(9,:) yb navig(9.:));grid 
xlabel('seconds');ylabel('feet');meta sdvz;pause 


!del sdvtheta.met 
pote, state(11,+)7t,navig(11,:));grid 
xlabel('seconds');ylabel('radians');meta sdvtheta;pause 


Idel sdvpsi.met 
palet, state(12,:) ,t,navig(12,:));0grid 
xlabel('seconds');ylabel('radians');meta sdvpsi;pause 


idel sdvpbias.met 
plat, navig(13,:));9grid 
xlabel('seconds');ylabel('radians/sec');meta sdvpbias; pause 


idel sdvqbias.met 
plot(t,navig(14,:));grid 
xlabel('seconds');ylabel('radians/sec');meta sdvqbias; pause 


!del sdvrbias.met 


L (L navig(15,:));grid 
xlabel('seconds');ylabel('radians/sec');meta sdvrbias;pause 
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/ * k k * k * * k k k k k k k k k k k k k H k k k k K X k K X K YEN KNEE KEKE KA 
* 


File MODEL.C 


C language source code for MODEL.MEX. Must be 
compiled within MEX subdirectory of MATLAB. 
It is used like a MATLAB function: 


function state = model(oldstate,inputs,dt) 


based) code in R. Boncal’s MS Thesis, 1987, 
NPS, Monterey, CA. It is based on modified 
equations of motion from NSRDC Report 2510 

June, 1967. 


* 
* 
* 
* 
* 
* 
* 
* 
* This code was translated from the DSL (FORTRAN 
* 
* 
* 
* 
* 
* Called by SIMUL9.M 

* 

* 


K KK HK K J K H H H K RRA KE ee 


#include <math.h> 
#include <stdio.h> 
#include "modelprm.h" 
#include "cmex.h" 


int model( double *state, double *oldstate, double 
*inputs, double *dt ); 


j= 
* This is the section of code recognized by MATLAB 
* It calls function MODEL 
x 
void user fcn( int nlhs, Matrix *plhs[], int nrhs, Matrix 
“prhis[ Jo) 
í 


double *oldstate, *inputs, *dt, *state; 


if (nrhs != 3) 

mex_error("Must be three input arguments."); 
if (nlhs != 1) 

mex error("Must be one output argument."); 


if (ROWS IN(0) !- 12 |l COLS IN(O) != 1) 

mex error("Previous state vector not correct size."); 
lf (ROWSSINOLy 1!2”4 |! | WCoOpsSs m1) I M 

mex error("Input vector not correct size."); 
if (ROWS _IN(2) != 1 |j; COLS IN(2) != 1) 


mex_error("Time interval must be a scalar.i); 


plhs[0] = create_matrix(12,1,REAL); 
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) 


State = OUT(0); 
oldstate = IN(0); 
inputs = IN(1); 
dt = IN(2); 


model(state,oldstate,inputs,dt); 


/* This code is the nonlinear model of the SDV */ 


int model( double *state, double *oldstate, double *inputs, 


double *dt ) 


{ 


i dicks 

double peer Ds O Ep. mea. PSL; 

double as) MAD rpm, delt; 

double mass, latyaw, norpit, re, termo; 

double Sap Sonn Meta, cad ret, Ctl, eps, xprop; 
double Weise tp 6 Vat [121 

double tmpl, tmp2, tmp3, tmp4; 

double coSs theta, sin theta, tan theta; 

double Cos phi; Sin phi, Cos psi; Sin- psi; 


u - oldstate[0]; 
V = oldstate[1]; 
w = olastate[2]; 
p = Oldstate[3]; 
q = oldstate[4]; 
É = Oldo tate], 
phi - oldstate[9]; 
theta - oldstate[10]; 
psi - oldstate[11]; 
dr - inputs[0]; 

us  - inputs[1]; 

c = inputs[2]; 

rpm = inputs[3]; 

delt = *dt; 


latyaw = norpit = 0.0; 
mass = weight/g; 
re = u*l/nu; 


Signu = FSIGN(u); 
Signn = FSIGN(rpm); 
if (fabs(u) < xltest) 
u = xltest; 
eta = 0.012*rpm/u; 
re = u*l/nu; 
cd0 = 0.00385 + 1.296e-17 * (re - 1.2e7)*(re - 1.2e7); 
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Cer 
ct 
eps 


0:/008*141/a0; 

ctl*eta*fabs(eta); 

=. .0+sionn/ signu*(sqre(ce+l.0)— 10) 
/ CSGEMCCE La lO 1.0); 

xprop = cdO*(eta*fabs(eta) - 1.0); 


/* 

* calculate the drag force, 

* integrate the drag over the vehicle 
z integrate using 4 terms 

its 


for (k=Q; koa n IPO R [ 
tmpl v1ig4[F]J r° 1; 
tmp2 w-g4[k]*q*1; 
ucf(k] = sart(tmpl*tmpl + tmp2*tmp2); 
if(1.0e-10 <= ucf[k]) Í 
term0 = ((rho/2.0)*(cdy*hh(k)*tmpl*tmpl 
* cdz*br[k)*tmp2*tmp2)) *gk4[4]*T/Auc PME 
latyaw += termO*tmpi; 
norpit += term0*tmp2; 


) 


/* 

* force equations 

= 

/* 

* common sub-expressions 

= 
tmpi-= (rTRho/2. 0); 
Lmp2 = tmpi*l; 
tmp3 = tmp2*1; 
tmp4 = tmp3*1; 


cos theta 
sin theta 


cos(theta); 
sin(theta); 


tan theta sin theta/cos theta; 
cos phi = cos(phi); 
Sin phi = sin(phi); 
cos _ psi = cos(psi); 
sin psl = sin(psi),; 
/* 
* longitudinal force 
i 
fp[0] = mass*v*r - mass*w*q + mass*xg*q*q 


im MASS XQG~ 77h Masse yOg p-G = Mass * Zo" oT 
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/* 
P/ 
fp[1] 


/* 
a 


íp[2) 


/ * 
B 


fp[3] 


/* 
* 


*/ 
fp(4] 


EMS” (APPO + Aqua. + XIL*PrE+xXpr*p*r) 

tap 27 ( aq YE+XVP*Y*pD+xvrI*v*r 

u*cgé Beds ds+xqdb*05)+ xrdr*u*r*adr) 

empl VVW EP wW xz xvdr*u*v*dr 

u*w* (xwds*ds+xwdb*db) + u*u*(xdsds*ds*ds+xdbdb*db*db 
xdrdr*dr*dr)) - (weight -boy)*sin theta 
tuD2*uwedsn*u*q*ds*eps 
tmpl*(xwdsn*u*w*ds-xdsdsn*u*u*ds*ds)*eps 
tapli*u*u*xprop; 


+ + + + + + + + + 


lateral force 


= -massi uir  mass*xg*p*g + mass*yg*r*r - mass*zg*q*r 
+ tmp3*(ypq*p*q + yqr*q*r)+tmp2*(yp*u*p + 

A NES + wwotwt*o cr wr*w*r) + tmpl* 

(yv*u*v + yyvw*y*w +ydr*u*tu*dr) -latyaw +(weight-boy)* 
cos thneta*sin phi+mass*w*p+mass*yg*p*p; 


normal force 


= mass*üu*q — mass*v*p - mass*xqg*p*r - mass*yg*q*r + 
meass*2q%D*p +mass*2g*g*q + tinp3* 

eo DIBFZDI*D*'I + Zrr*r*r)- --tnunp2*(zq*u 

O ZYP D + 2vr*v*r) —tmol*(zm*u"m 

+ 2vvtvitvtutu* (zds*ds+zdb*db) )- 
norpit+(weight-boy)*cos theta*cos phi 

Teme *comeu*Oq*eps +tmpi* (2awn*u* pe +zdsn* 

ui *dis)*eps; 


roli force 


zac tl tO A ee 1X Vege +1yz*q*q 1iyz*r*r +ixz*p*q+ 
mass*yg*u*q -mass*yg*v*p -—mass*zg*w*p+tmp4*(kpq* 

mee eee or) xtmps*whp*u*p +kr*u*r + kvq*v*qgq + 
kwp*w*p + kwr*w*r) -tmp2*(kv*u*v + kvw*v*w) - 
(yg*weight - yb*boy)*cos theta*cos phi - (zg*weight - 
ZO"BOY coSs theta*sin phi + tmp3*kpn*u*p*eps+ 
tmp2*u*u*tkprop -mass*zg*u*r; 


pitch force 


= -Dr +iz*p*r +1Xy*G*rC -1YZ*D*q -ixz*p*p Tix2*r*r- 
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mass*xg*u*q + mass*xg*v*p + mass*2g*v*r - mass* zo 3 
+ tmp4*(mpp*p*p +mMpr*p*r tM + 

tmp3* (mqtu*q + mvp*v*D r Mvr E 1 

tmp2* (mw*u*w+mvv*v*v+u*u*(mds*ds+mdb*db)) + norpit - 
(xg*weight-xb*boy)*cos_theta*cos phi+tmp3*mqn*u*q*eps 
+ tmp2*(mwn*u*w+mdsn*u*u*ds)*eps- 
(zg*weight-zb*boy)*sin theta; 


i uA 
/* 
À yaw force 
a 
fp[5] = -iy*p*q +ix*p*q +ixy*p*p -ixy*q*q kiyz*žp*ri 1x2 


mass*xg*u*r + mass*xg*w*p - mass*yg*v*r + mass*yg*w*q 
+ tmp4* (npq*p-Gq t ngr*qłźr) temp? (0p I 

nr*u*r + nvq*v*q I1nDnwD*W*D F AW riw ir i mp2 (n 

u*v + nvw*v*w + ndr*u*u*dr) - latyaw + (xg*weight - 
xb*boy)*cos theta*sin_phi+(yg*weight)*sin theta 
+tmp2*u*u*nprop-yb*boy*sin theta; 


/* 
* now compute the f(0-5) functions 
"Z 
for (j=0; j<6; ++j) 
for (f[j)J]=0.0,K=0; k<6; +k) 
f[j] += xmminv[j][k]*fp[k]; 
i 
* the last six equations come from the kinematic 
g relations 
1 7: 
/* 
* inertial position rates f(6-8) 
i 
f[6] = u*cos_ psi*cos theta + v*(cos psi*sin theta‘ 
sin_phi - sin psi*cos phi) + w*(cos_psi*sin_theta* 
cos_phi + sin_psi*sin phi); 
£(7] = u*sin_psi*cos theta + v*(sin_psi*sin_theta* 
sin phi + cos psi*cos phi) + w*(sin psi*sin_theta* 
cos phi - cos psi*sin phi); 
f[8] = -u*sin theta -v*cos theta*sin phi 
+w*cos theta*cos phi; 
x 
* euler angle rates f(9-11) 
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me) = po teq*sin phi*tan theta + r*cos phi*tan theta; 
f[10] = gq*cos phi - r*sin_ phi; 
Éf[11] = q*sinephsa/eos theta + r*cos phi/cos theta; 

* 

Z Simpson’s rule integration 

E 


for (j=0; j<12; j++) 
state[j] = oldstate[j] + delt * f[j]; 


return 0; 
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/ k Kk K Kk Kk H K KK K KKK KK KK IAKA I E E RENK A AER 
* 


File MKABMEX.C 


C language source code for MKABMEX.MEX Must be 
compiled within MEX subdirectory of MATLAB. 
It is used like a MATLAB function: 


function [A,B] = mkABmex(state,inputs) 


* 

* 

* 

* 

* 

* 

* 

* 

* This program makes a linearized model in MATLAB 
* for the SDV around some [x0] and [u0] which are 
* input parameters. The vehicle mass matrix and 
* hydrodynamic coefficients are in "modelprm.h" 

* 

* 

* 

* 

* 

* 

* 


This code is based on Taylor series linearization 
of the equations of motion in MODEL.C 


Called by SIMUL9.M 


k k * k k k Kk k * k k * k k e K K H k k k ke k k ke K ke ke sk ke k k k ke k k e * * k k * * % / 


#include <math.h> 
tinclude "tcmex.h" 
#include "modelprm.h" 


void makeab( double *A, double *B, double *state, double 
*inputs); 


/* 
* Code recognized by MATLAB. It calls MAKEAB 
A 


void user fcn( int nlhs, Matrix *plhs[], int nrhs, Matrix 
*prhs[] ) 


double *state, *inputs, *a, *b; 


if (nrhs !-» 2) 
mex error("Must be two input arguments"); 
if (nlhs !-2 2) 
mex error("Must be two output arguments"); 


if (ROWS IN(0) !- 12 j| COLS IN(O) != 1) 
mex error("Initial state vector must have 12 states"); 
if (ROWS IN(1) != 4 |! COLS IN(1) != 1) 


mex error("Input vector must have 4 inputs"); 


plhs[0] create matrix(2 2 REAL); 
pihs[1] create matrix(12,4,REAL); 
a - OUT(0); 
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ba OUT (1); 


state 
inputs 


IN(0); 
= NG 


makeab(a,b,state,inputs); 


} 
y= 
This 


code generates the linearized A and B matrices 


motion for the vehicle. 


/ 


* 
* from the partial derviatives of the equations of 
* 
* 


void makeab( double *A, double *B, double *state, double 


*inputs 


int 

double 
double 
double 
double 
double 
double 
double 
double 
double 


) 


iE; 
a(12][12], D[(6](4]; 
aa[12][12],bb[12]([4]; 
u0, v0, w0, pO, GO, r0, pPphi0, theta0, psio0; 
dr; ds, db, rpm; 
mass, latyaw, norpit, re, term0; 
etarecdo/ Et,  ctl, eps, xprop; 
tmpl, tmp2, tmp3, tmp4, tmp5; 
cos theta, sin theta, tan theta; 
cos phi, sin phi, cos psi, sin psi; 


/* assign some common variable names */ 


u0 = state[0]; 

v0 2 state[1]; 

wO - state[2]; 

pO = state[3]; 

q0 = state[4]; 

r0 = state[5]; 

phi0 = state[9]; 
thetaO State one 
psi0 = state[ 11]; 
dr inputs[0]; 


db 


ds = inputs(1]; 
= inputs[(2]; 


rpm - inputs[3]; 


cos theta 
sin theta 


cos(theta0); 
sin(theta0); 


tan_theta = tan(theta0); 
cos phi - cos(phi0); 
sin phi - sin(phi0); 
cos psi = cos(psi0); 
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sin psi - sin(psi0); 


if (thetaO0 == 0.0) ( sin theca = 0.0 tans thetas No 
if (phi0 == 0.0) sin_phi = 0.0; 
if (psi0 == 0.0) sin psi = 0.0; 


/* linear propulsion model */ 


eta = 0.012* npn uc, 

re = u0*l/nu; 

cdo = 0.00385+(1.296e-17)*(re-1.2e7)*(re-1.2e7); 
ct = 0.008*1*l*eta*abs(eta)/a0; 

ctl = 0.008*1*1/a0; 

eps = -1+(sgqrt(ct+1)>)-1)/(sgqrt(ct1+1)-1); 

xprop = cdo*(eta*abs(eta)-1); 


/* assign commonly used values */ 


tmpl = rho/2*1; 
tmp2 - tmpl*l; 
tmp3 - tmp2*1; 
tmp4 = tmp3*1; 
tmp5 = tmp4*1; 
mass = weight/g; 


/* initialize a, b, aa, and bb */ 


for (J9; a2 15) ph) 
for (K0; kI? TEF?) 
a[3][k] = 0.0; 


Lor (30, 49559 55) 
for (k=O; k<4; k++) 
Sig O 


for (3=0; 3<12; 3++) 
tor O EA o) 
aa[j][k] = 0.0; 


tor (20 EU 7777) 
for (k=0; k<4; k++) 
bb[j][k] = 0.0; 


/* Build the 12x12 a matrix  */ 


a[0][0]= tmp3*( xqds*ds*q0+xqdb/2*db*q0+xrdr *r0*dr ) + 
tmp2* (xvdr*v0*dr+xwds*ds*w0+xwdb/2*w0*db+ 
2*u0* (xdsds*ds*ds+xdbdb/2*db*db+xdrdr*dr*dr)); 
a[0j[0]» a[0][0] + tmp3*xgdsn*q0*ds*eps+tmp2*(xwdsn*w0*ds+ 
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2*xdsdsn*u0*ds*ds) *eps+tmp2*u0*xprop+tmp3* 
xqdb/2*db*q0+tmp2*xwdb/2*db*w0+tmp2*u0* 
xdbdb/2*db*db; 

a[0](1]= mass*r0+tmp3*(xvp*pUO+xvr*r0) 
+tmp2* (2*xvv*v0+xvdr*u0*dr); 

a[0][2]= -mass*q0+tmp3*xwq*q0+tmp2*(2*xww*w0+xwds*ds*u0+ 
xwdb*db*u0+xwdsn*u0*ds*eps); 

a[0]([3]= —-mass*yg*q0-mass*zg*r0+tmp4*(2*xpp*p0O0+xpr*r0) 
Eemos *xXvp* v0; 

a[0][4]» -mass*w0-*2*mass*xg*q0-mass*yg*pO-tmp4*2*xqq*qO- 
tmp3* (xwq*w0+xqds*ds*u0+xqdb/2*db*u0)+ 
tmp3*xqdsn*u0*ds*epst+tmp3*xqdb/2*db*u0; 

a[0](5]=> mass*v0+2*mass*xg*r0-mass*zg*p0+tmp4*(2*xrr*r0+ 
rL *D0)+tmp3* (xvr*v0+xrdr*u0*dr); 

a[0][10]=-(weight-boy)*cos theta; 


a[1][0]=-mass*r0+tmp3*(yp*p0+yr*r0)+tmp2*(yv*v0+ 
2*ydr*u0*dr); 

a[1](1]= tmp3*yvq*q0+tmp2*(yv*u0+yvw*w0); 

a[1](2]= mass*p0+tmp3*(ywp*pO0+ywr*r0)+tmp2*yvw*v0; 

a[1](3]= mass*w0-mass*xg*q0+2*mass*yg*p0+tmp4*ypq*qu+ 
tmp3* (yp*u0+ywp*w0) ; 

a[1][4]=-mass*xg*p0-mass*zg*r0+tmp4*(ypq*p0+ygr*r0)+ 
tmp3*yvq*v0; 

a[1][5]=-mass*u0+2*mass*yg*r0-mass*zg*q0+tmp4*yqr*q0+ 
tmp3*(yr*u0+ywr*w0); 

a[1](9]» (weight-boy)*cos theta*cos phi; 

a[1][10]2-(weight-boy)*sin theta*sin phi; 


a[2][0]= mass*q0+tmp3*zg*q0+tmp2*(zw*w0+2*u0*zds*ds+ 
2*u0*zdb/2*db+(zwn*w0+2*zdsn*u0*ds)*eps)+tmp3* 
Z2gn*q0*eps+tmp2*2*u0*zdb/2*db; 

a[2][1]=-mass*p0+tmp3*(zvp*pO0+zvr*r0)+tmp2*2*zvv*v0; 

a[2][2]= tmp2*(zw*u0+zwn*u0*eps); 

a[2][3]=-mass*v0-mass*xg*r0+2*mass*zg*p0+tmp4*(2*zpp* 
p0+zpr*r0)+tmp3*zvp*v0; 

a[2][4]= mass*u0-mass*yg*r0+2*mass*zg*q0+tmp3*zq*u0+ 
tmp3*zqn*u0*eps; 

a[2][5]=-mass*xg*p0-mass*yg*qO+tmp4*(zpr*p0+2*zrr*r0)+ 
II 32 Z2vr*v0; 

a[2][9]2-(weight-boy)*cos theta*sin phi; 

a[2][10]2-(weight-boy)*sin theta*cos phi; 


a[3][0]= mass*yg*q0+mass*zg*r0+tmp4*(kp*p0+ 
kr*r0)+tmp3* (kv*v0+2*u0* (kdb/2*db-kdb/2*db) ) + 
tmp3*u0*kprop+tmp4*kpn*p0*eps; 

a[3][1]=-mass*yg*p0+tmp4*kvq*q0+tmp2*(kv*u0+kvw*w0); 
a[3][2]=-mass*zg*p0+tmp4*(kwp*pO0+kwr*r0)+tmp3*kvw*v0; 
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a[3][3]=-ixy*r0+ixz*q0-mass*yg*v0-mass*zg*w0+ 
tmp5*kpq*q0+tmp4* (kp*u0+kwp*w0 ) ; 
a[3][4]=-iz*r0+iy*r0+2*iyz*q0+ixz*p0+mass*yg*u0+ 
tmp5* (kpq*p0+kqr*r0)+tmp4*kvg*v0; 
a[3](5]=-iz*q0+iy*q0-2*iyz*r0+mass*zg*u0+tmp5*kqr*q0+ 
tmp4* (kr*u0+kwr*w0); 
a[3][9]=-(yg*weight-yb*boy)*cos_theta*sin phi- 
(zg*weight-zb*boy)*cos theta*cos phi; 
a[3][10]=-(yg*weight-yb*boy)*sin_theta*cos phi+ 
(zg*weight-zb*boy)*sin theta*sin phi; 


a[4][0]=-mass*xg*q0+tmp4*mq*q0+tmp3*mw*w0+tmp3*u0* 
(mds*ds+mdb/2*db)+tmp4*mqn*q0*eps+ 
tmp3* (mwn*w0+2*mdsn*u0*ds)*eps+tmp3*u0*mdb/2*db; 
a[4)])[1]= mass*xg*p0+mass*zg*r0+tmp4*(mvp*p0O0+mvr*r0) 
+EMPS MUDO 
a[4)][2)]=-mass*zg*q0+tmp3*mw*u0+tmp3*mwn*u0*eps; 
a[4][3]=-ix*r0+iz*r0-iyz*q0-2*ixz*p0+mass*xg*v0+ 
tmp5*( 2*mpp*p0+mpr*r0)+tmp4*mvp*v0; 
a[4][4]= ixy*r0-iyz*p0-mass*xg*u0-mass*zg*w0+tmp4*mq*u0+ 
tmp4*mqn*u0*eps; 
a[4][5]=-ix*p0+iz*p0+ixy*q0+2*ixz*r0+mass*zg*v0+ 
tmp5* (mpr*p0+2*mrr*r0)+tmp4*mvr*v0; 
a[4][9]=(xg*weight-xb*boy)*cos_theta*sin phi; 
a[4][10]=(xg*weight-xb*boy)*sin _theta*cos phi- 
(zg*weight-zb*boy)*cos theta; 


a[5][0]=-mass*xg*r0+tmp4*(np*pO0+nr*r0)+tmp3* 
(nv*v0+2*ndr*u0*dr)+tmp3*u0*nprop:; 
a[5][1)]=-mass*yg*r0+tmp4*nvq*q0+tmp3*(nv*u0+nvuw*w0); 
a[5][2]= mass*xg*p0+mass*yg*q0+tmp4* (nwp*p0+nwr*r0)+ 
Emp 3 *nvwey 0; 
a[5][3]=-iy*q0+ix*q0+2*ixy*p0+iyz*r0+mass*xg*w0+tmp5*npq*q0+ 
tmp4*(np*u0+nwp*w0); 
a[5][4]=-iy*p0+ix*p0-2*ixy*q0-ixz*r0+mass*yg*w0+ 
tmp5*(npq*p0Fnqgr*r0)+Ftmp4*nvqgq*v07; 
a[5][5]= iyz*p0-ixz*q0-mass*xg*u0-mass*yg*v0+ 
tmp5*ngr*gq0+tmp4*(nr*u0+nwr*w0); 
a[5][9]=(xg*weight-xb*boy)*cos_theta*cos phi; 
a[5][10]2-(xg*weight-xb*boy)*sin theta*sin phi- 
(yg*weight-yb*boy)*cos theta; 


a[6][0]= cos psi*cos theta; 
a[6][1]= cos psi*sin theta*sin phi-sin psi*cos phi; 
a[6][(2]2» cos, psi*sin, theta*cos phi-sin psi*sin, phi; 
a[6][9]=v0*cos _psi*sin theta*cos phi+v0*sin psi* 
sin phi-w0*cos psi*sin theta*sin phi+ 

wO*sin psi*cos phi; 
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a[6][10]=-u0*cos_psi*sin _theta+v0*cos_psi*cos theta* 
sin phi*wO*cos psi*cos theta*cos phi; 
a[6][11]2-uO*sin psi*cos theta-vO*sin psi*sin theta* 
sin phi-vO*cos psi*cos phi-wÜ*sin psi* 
sin theta*sin phi-*wO*cos psi*sin phi; 


a[7][0]2» sin psi*cos theta; 
a[7][1]» sin psi*sin theta*sin phi-«cos psi*cos phi; 
a[7][2]» sin psi*sin theta*cos phi-cos psi*sin phi; 
a[7][9]= vO0*sin psi*sin _ theta*cos phi-v0*cos psi* 
sin phi-wO*sin psi*sin theta*sin phi- 
wO*cos psi*cos phi; 
a[7][(10]=-u0*sin _psi*sin _theta+v0*sin _ psi*cos theta* 
sin phi+w0*sin psi*cos theta*cos phi; 
a[7][(11]=u0*cos_psi*cos _theta+v0*cos psi*sin theta* 
sin phi-v0*sin psi*cos phi+w0O*cos psi* 
Sin theta*cos phitw0*sin psi*sin phi; 


a[8][0]=-sin_theta; 

a[8][1]= cos_theta*sin phi; 

a[8][2]= cos theta*cos phi; 

a[8][9]2vO*cos theta*cos phi-wO*cos theta*sin phi; 

a[8][10]=-u0*cos_theta-v0*sin theta*sin phi- 
wO*sin theta*cos phi; 

a[9][3]=1; 

a[9][4]=sin _phi*tan theta; 

a[9][5]2cos phi*tan theta; 

a[9][9]=q0*co0s_phi*tan theta-r0*sin phi*tan theta; 

a[9][10]=q0*sin phi/cos _theta*l/cos theta+ 
r0*cos_phi/cos_theta*1l/cos theta; 


a[10](4]=c0s_phi; 
a[10][5]-2-sin phi; 
a[10][9]=-q0*sin phi-r0*cos phi; 


a[11](4]=sin phi/cos theta; 

a[11][5]2cos phi/cos theta; 

a[11][(9]=q0*c0s_phi/cos_theta-r0*sin phi/cos theta; 

a[11][10]2q0*sin phi/cos theta*tan theta-* 
r0*cos_phi/cos_theta*tan theta; 


/* Build the 12x4 b matrix  */ 
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b(Oj)(Oj= tmp3*xrdr*u0*r0+tmp2* (xrdr*u0*v0+u0* u0*2*xdrdr aay 
b[0][1]= u0*q0*xgds+u0*w0*xwds+u0*u0*2*xdsds*ds+ 
tmp3*xqdsn*u0*q0*epst 
tmp2* (xwdsn*u0*w0+2*xdsdsn*u0*u0*ds ) *eps; 
b[0][2]= u0*q0*xgqdb+u0*w0*xwdb+2*u0*u0*xabdb*db; 
b[0][3]= tmp2*0.012*cdo*0 12 pm 


b[1](0]= tmp2*ydr*u0*u0; 


b(2][1]=u0*u0*zds*tmp2+tmp2*zdsn*u0*u0*eps; 
b[2][2]=u0*u0*zdb*tmp2; 


b[3][2]= 0; 


b[ 4] 


[i tmp3* (u0*u0*mds+mdsn*u0*u0*eps ); 
b[4][2 


] > 
|= tmp3*u0*u0*mdb; 
bD[5][0]= tmp3*ndr*u035u0:; 


/* Multiply the appropriate parts of both by the inverted 
* mass matrix 
* 


* inv(mass matrix)*df/dx 


for (10) TOMES) 
Tor) -0 P) 6 
for (k=0; K<6; `k++) 
: aa[iJ[j] *- xmminv[iJ][k]*a[k][j]; 
* 
*  inv(mass matrix)*df/dz 


"Z 


for (120: 9166 9225) 
for (JO R SLI, 
for (k=0; k<6; k++) 
aa[(i][3] += xmminv(i](k]*a[k][3]; 


for (1-60 e) 
for (J; J12; ji p) 
aa A Ae 


/* 
*  inv(mass matrix)*df/du 
x 
for (i=0; i<6; itf) 
for (3=0; j«44; j-*-*) 
for (k=0; k<6; k++) 
AAA A A ks eee ae PIE: 
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/* 
* reorganize the matrices for use by Matlab 
* which stores matrices columnwise vice rowwise 


E 


for (j-70; j«412; j**) 
lor (r0 12: 2155) 
A eara ge 
for (j=0; 3<4; j++) 
ror UA IA) 
Bee) A AO ASA 


Jl] 


f Noe ke k k k k k k k k k k k k k k k k k k k k k k k k k k Kk k k k k k k k k k k k k K 
* 


Flle MODELPRM.H 


* 

* 

* This file contains all of the parameter 

* coefficients used by the files MODEL.C. 

* and MAKEAB.C. 

* These coefficients were obtained from 

* NCSC Technical Memorandum 231-78, June 78 
* 
* 
* 


kk ck f k k K k K k k k K K He de 76 HKN TA ee 


/* 

* longitudinal hydrodynamic coefficients 

B 
const double 
xpp = 7.0e-3, xqq = -1.5e-2, xrr = 4.0e-3, 
XPE = 7.5e-4, xudot= -7.6e-3, xwq = -2.0e-1, 
XVp = -3.0e-3, xvr = 2.0e-2, xqds = 2.5e-2, 
xqdb = =2.6e-3, xrdr = -1.0e-3, xvv = 5.3e-2, 
XWW = 1.7e-1, xvdr = l.7e-3, xwds = 4.6e-2, 
xwdb = 1.0e-2, xdsds= -1.0e-2, xdbdb= -8.0e-3, 
xdrdr = -1.0e-2, xqdsn= 2.0e-3, xwdsn = 3.5e-3, 
xdsdsn= -1.6e-3; 
/* 

* lateral hydrodynamic coefficients 

E 
const double 
ypdot = 1.2e-4, yrdot = 1.2e-3, ypq = 4.0e-3, 
yar = -6.5e-3, yvdot = -5.5e-2, yp = 3.0e-3, 
yr = 3.0e-2, yvq = 2.4e-2, ywp = 2.3e-1, 
ywr = -1.9e-2, yv = -1.0e-1, yvw = 6.8e-2, 
ydr = 2.7e-2, cdy = 3.5e-1; 
/* 

* normal hydrodynamic coefficients 

zr 
const double 
zqdot = -6.8e-3, zpp = 1.3e-4, zpr = 6.7e-3, 
ZEL = -7.4e-3, zwdot= -2.4e-l, zq = -1.4e-1, 
ZVp = -4,8e-2, zvr = 4.5e-2, zw = -3.0e-1, 
ZVV = -6.8e-2, zds = -7.3e-2, zdb = -2.6e-2, 
zqn = -2.9e-3, zwn = -5.le-3, zdsn = -1.0e-2, 
caz = 110% 
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/* 
E roll hydrodynamic coefficients 


E / 


const double 


kpdot = -1.0e-3, krdot = -3.4e-5, kpq = -6.9e-5, 
kqr = 1.7e-2, kvdot = 1.3e-4, kp = -1l.le-2, 
kr = -8.4e-4, kvq = -5.le-3, kwp = -1.3e-4, 
kwr = 1.4e-2, kv = 3.le-3, kvw - -1.9e-1, 
kpn = -5.7e-4, kdb = 0.0; 

/* 

* pitch hydrodynamic coefficients 

un 
const double 
mqdot - -1.7e-2, mpp = 5.3e-5, mpr = 5.0e-3, 
mrr = -2.9e-3, mwdot = -6.8e-3, mq = -6.8e-2, 
mvp 2026-5 F mu r = 1.7e-2, mw = 1.0e-1, 
mv v = -2.6e-2, mds = -4.1e-2, mdb = 6.9e-3, 
mqn = -1.6e-3, mwn = -2.9e-3, mdsn = -5.2e-3; 
/* 

* yaw hydrodynamic coefficients 

E / 
const double 
npdot = -3.4e-5, nrdot = -3.4e-3, npq = -2.1e-2, 
ndr = 2.7e-3, nvdot = 1.2e-3, np = -8.4e-4, 
nr = -1.6e-2, nvq = -1.0e-2, nwp = -1.7e-2, 
nwr = 7.4e-3, nv = -7.4e-3, nvw = -2.7e-2, 
nar = -1.3e-2; 

/* 

* 


mass characteristics of the flooded vehicle 


= 


const double 


weight = 12000.0, boy = 12000.0, vol = 200.0, 
xg = 0.0, yg = 0.0, zg = 0.20; 
xb = (5:05 zb = 0"0- 7 1x4 = 1500.0; 
ly = 10000.0, iz = 10000.0, ixz = -10.0, 
iyz = -10.0, ixy = -10.0, yb = 0.0, 
l= 17747; rho = 1.94,q - 3222" 
nu = 8.47e-4,a0 = 2.0; kprop = omor 
nprop = 0.0, Aces =Po., 

degrud = 0.0, degstn = 0.0; 


FES 


/* 
* define length fractions for gauss quadrature terms 


A 


const double 
g4[] = { 0.069431844, 0.330009478, 0.669990521, 
0.930568155 }, 
gk4[] = { 0.1739274225687, 0.3260725774312, 
0.3260725774312, 0.1739274225687 |; 


/* 
* define the breadth bb and height hh terms for the 
$ integration 
=) 


const double 


br[] = {75.7/12.0, ?5.7/12.0, 75.7712 30755087 128012 
hh[] = {16.38/12.0, 31.85/12.0, 31.85/4102905923 4767 eee 
| * 
* assemble inverted mass matrix 
s 


const double 
xmminv[6](6] = 
[ 0.2431le-2, 0.2701e-8, 0.1899e-5, 0.1649e-7, 
-0.5023e-5, 0.3243e-8 ), 


{ 0.2679e-8, 0.1537e-2, 0.5593e-8, 0.4276e-4, 
-0.14792=7 ODE eni ha 

{ 0.1899e-5, 0.5639e-8, 0.6293e-3, 0.3443e-7, 
-0.1049e-4, 0.6770e-8 }, 


( 0.1649e-7, 0.4321e=4, 073443637, 0:/32942_ 37 
-0.9106e-7, -0.1049e-5 }, 
[ -.5023e-5, -.1491e-7, -.1049e-4, -.91066-7, 
0027739 O S 7) 
( 0.3243e-8, 0.1057e-4, 0.6769e-8, -.1052e-5, 
-0.1790e-7, 0.6561le-4 } 
y; 
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% % % % % % % % % % % $ $ % % % % % % $ % $ % $ $ $ $ $ % % % $ $ % % % % % % % % % % $ $ $ 
Flle GETMEAS2.M 


simulates accelerometer and rate gyro 
measurements using the nonlinear model as the 
vehicle. Additive white Gaussian noise included 


Called by SIMUL9.M 


% 
% 
% 
% 
% 
% 
% 
% 
% 
% Ver.3 Includes gyro biases, and depth rate meas. 
% no speed log measurement 

% 

% 


EEEEEEEEEEEEEEEEEEEEE EEE EEE EEE EE EEEEEEEEEEEES 
function [anglerate,Z,Zdot,accel,hdg]=getmeas3(new, Zold,dt) 
rand('normal'); 


$ depth measurement 
Z = new(9)+le-3*rand; 


% time rate of change of depth 
or = (Z - Zold)/dt; 


$ simulated noise and bias on rate gyro 
Vp=0.003*rand; 

Euz020053*rand; 

Vr=0.003*rand; 

pBias - 0.10; qBias -0.200; rBias = -0.25; 


$ rate gyro readings 

anglerate(1,1)=new(4) + pBias + Vp; 
anglerate(2,1)=new(5) + qBias + Vq; 
anglerate(3,1)=new(6) + rBias + Vr; 


% accelerometer measurement of gravity (down is positive) 
E0050; 32.2]; 


$ transform gravity vector to body coordinates to subtract 


phi=new(10); % Euler roll angle 
the=new(11); % Euler elevation/dive angle 
psi=new(12); % Euler azimuth/heading 


% calculate the tranformation matrix 


Pi, 1) = cos(psi)*cos(the); 
R(2,1) = cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); 
R(3,1) = cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); 
R(1,2) = sin(psi)*cos(the); 
R(2,2) = cos(psi)*cos(phi)+sin(psi)*sin(the)*sin(phi); 


ES 


R( 372) -cos(psi)*sin(phi)+sin(psi)*sin(the)*cos(phi); 


R(1,3) i -sin(the); 
R(2,3) = cos(the)*sin(phi); 
R(3,3) = cos(the)*cos(phji); 


accel = -R*g + 0.00025*rand(3,1); 


$ simulated accelerometer dead zone 
lf (abs(accel(1)) < 0.4 
accel(1) = 0.0; 
end; 
if (abs(accel(2)) < 0.4) 
accel(2) = 0.0; 
end; 


hdg = psi; 
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EEEEESEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEESY 
File MAKMEAS3.M 


transforms the observer states into measurements 
using nonlinear measurement equation. 


$ 
$ 
% 
$ 
$ 
$ 
% Called by SIMUL9.M 

1 

$ Ver.3 corresponds with getmeas2 

$ 

% % % % % % $ % % % $ % % $ % % % $ % % $ % % $ $ % $ $ $ $ $ $ % $ $ $ $ $ $ % $ $ $ % $ % 


function [modmeas]-makmeas2(new,old,dt) 


modmeas = zeros(9,1); 

modmeas(1) = new(4) + new(13); $ rate gyro 
modmeas(2) = new(5) + new(14); $ and bias estimate 
modmeas(3) = new(6) + new(15); 

modmeas(4) = new(9); % depth 

modmeas(5) = (new(9)-old(9))/dt; % depth dot 


$ gravity (down is positive) 
g=[0;0; 32.2]; 


% transform gravity vector to body coordinates to subtract 


phi=new(10); % Euler roll angle 
the=new(11); % Euler elevation/dive angle 
psi=new(12); $ Euler azimuth/heading 


$ calculate the tranformation matrix 
R(1,1)=cos(psi)*cos(the); 
R(2,1)=cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); 
R(3,1)=cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); 
R(1,2)=sin(psi)*cos(the); 
R(2,2)=cos(psi)*cos(phi)+sin(psi)*sin(the)*sin(phi); 
R(3,2)=-cos(psi)*sin(phi)+sin(psi)*sin(the)*cos(phi); 
R(1,3)=-sin(the); 

R(2,3)=cos(the)*sin(phi); 

R(3,3)=cos(the)*cos(phi); 


modmeas(6:8)=-R*g; $ accelerometer meas 


modmeas(9)= psi; 
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% % % % $ % % % $ % % % $ % % $ % % $ % % $ $ % $ $ $ % $ $ $ $ $ $ $ $ $ % $ $ % % $ % % $ 
% 
Flle KALM.M 


% 

% 

% Time varying Kalman Filter subroutine 

% for predictor-corrector formulation of filter 
% 
% 
% 
% 


Called by SIMUL9.M, AUVSIM.M, MAKEK.M 
% $% % % $ % $ $ $ % $ $ $ $ % $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ % $ $ $ 
function [K,Pnew]= kalm(Phi,Del,C,Pold,W,V) 


% update Pold 
P=Phi*Pold*Phi'+Del*W*Del'; 


% calculate new gain matrix 
K=P*C'*inv([C*P*C'+V]); 


% complete update of P with new K matrix 
Pnew=(eye(Phi)-K*C)*P; 


APPENDIX B 


Program Listing for Experimental NPS AUV II Study 
% % $ % % % $ $ % $ $ % $ $ % % $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ % $ $ $ $ $ % $ $ % 
File AUVSIM7 .M 


Structured as SIMUL.M but this uses 
recorded data from the NPS AUV 2 vehicle. 


Calls AUV2.M, GETMEAS.M, MAKMEAS.M 
MAKEK.M, GETK.M 


Modified 18 Nov 91 
Ver.2 added rate gyro bias terms to observer 


Ver.3 removed speed log measurement 
and added accelerometer dead zone 


Ver.4 modified to use recorded vehicle data 
and simulating accelerometer data from 
vertical gyro data 


Ver.5 using gyro data directly and added bias term for 
PECADO 


Ver.6 augment state to estimate 
error in u using an input rpm bias as a state 


% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% 
% Ver.7 using piecewise constant Extended Kalman filter 
% gain matrix. (and drop rpm bias) 

% 

% 


% $ % % % % $ % % $ % $ $ $ $ % $ $ $ $ $ $ $ $ $ $ $ $ $ % $ $ $ $ $ % $ $ $ $ $ $ $ % $ 


% dt = sample interval 


% load recorded data into memory. File obtained from 
% NPS AUV Group 

load dat8262.d 

data = dat8262; 

clear dat8262; 


% Set initial state based on data and estimated conditions 
X = data(1,2); 
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Y data(1,3); 


Z = data(1,4); 
rpm = 550; 

ud = 050 

wO = 0.0; 
pitchrate - data(1,9); 
pitch = data(1,6); 
heading = data(1,7); 
phi0 = 0.0; 

the0 = pitch; 
psi0 = heading; 


initial state = 
[u0;0;w0;0;pitchrate;0;xX;Y;2; 0; pitch; heading)]- 


% Set sample interval and run time based on data set 
clt data(2,1)-data(1,1); 
TE data(length(data),1)-data(1,1); 


$ Set up disturbance and measurement noise covariance 

% matrices 

$ (prepare to leave out X and Y terms from Kalman filter) 
Q = zeros(14); 

0(1:10,1;10) = O0S05*eve(1057 


Q(1,1) = 1.0; 
Q(9,9) = .05; 
Q(11:14,11:14) = 0.0005*teye(4); $ gyro bias rand walk 


$ Measurement noise covariance matrix 

$ off diagonal terms for correlation between forward 
$ speed, pitch and depth rate. 

R-.01*eye(7); 


R(1,5) = 0 001; 
R(5,1) = R(1,5); 
R(6,6) = .005; 

R(5,5) = 0.05; 

R(5,6) = 0.001; 
R(6,5) F rh O), 
RT DIO 


$ Initialize vectors for simulation 

imax -Tf/dt + 1; 

navig = zeros(16,1imax); 

navig(:,1) = [initial state;0;0;0;0]; 

% Make block row matrix of steady state 

% Extended Kalman filter gain matrices for table lookup 
% the gain will be a function of u and theta 


uRange = [UIr5]/ 27016 
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emeRange = {-0.05;-0.025;0.0;0.025;0.05]; 
allK = makeK(uRange,thtRange,Q,R,dt); 
meas = zeros(7,1); 


disp(’Navigation Begins...’) 
tO-clock; 


for (i=1:imax) 


rpm = ((data(i,17)+data(i,18))/2); 
nputm =... 
fdata(i,11);-data(i,il);data(i,12);-data(i,12);rpm); 


Bhat = navigl,i); 
thetahat = navig(11,1); 


% Look up proper steady state K matrix for the current 
% estimated state 
K » getK(allK,uhat,thetahat,uRange,thtRange); 


$ Format measurement from data file 
[rategyro,2,Zdot,pitch,hdg]-2getmeas5(data(1,:),2,dt); 
meas - [rategyro;2;Zdot;pitch;hdg]; 


$ Calculate next navigator prediction 
Wavig(1:12,1+1) 2 auv2(navig(1:12,3),inputm,dt); 
navig(13:16,39*1) = navig(13:16,1); 


% Generate predicted measurements 
modmeas(:,i+1) - makmeas3(navig(:,i-*1),navig(:,i),dt); 


$ Calculate correction using Kalman gain 
esterr(:,i) - K*(meas-modmeas(:,i-1)); 


$ Correct navigator prediction 
udvug(o:,;itl)ses. 
navig(:,i*l)*[esterr(i1:6,1);0;0;esterr(7:14,1)]; 
end; 
etime(clock,t0) 
pause 
axis([0 117 0 65]; 
$ Generate graphical output 
!del expxy.met; 
moer (dacta(:72)+20 data(: ; 39+15,navig(7,:)+20,navig(8,:)+15); 


12.1 


grid;xlabel('X - feet’ );ylabell( “Y =steet ’)- Grid bance 
meta expxy; 


axis; 
t = data(1,1):dt:data(length(data),1)+dt; 


Idel expur mer, 

plot (data(:,1), data(:,15),t navag@l >>) cmd, 
ylabel('feet/sec');xlabel('seconds');pause; 
meta expu; 


idel expv.met; 

plot(t, navig(2,:) )¢ gerd, 
xlabel('seconds');ylabel('feet/sec');pause; 
meta expv; 


ldel expw.met; 

plot(t;jnaviqg(372)) ord; 
xlabel('seconds');ylabel('feet/sec');pause; 
meta expw; 


!del expp.met; 
plot(data(:,l),data( 8) t navig(47;)) orig; 
xlabel('seconds');ylabel('radians/sec');pause; 
meta expp; 


idel expq.met; 

plot(data(:,1),data(: ,9),t,mavigts, m gr rd, 
xlabel('seconds');ylabel('radians/sec');pause; 
meta expq; 


!del expr.met; 
plot(data(:,1),data(:,10),t,navig(6,:)y);grid; 
xlabel('seconds');ylabel('radians/sec');pause; 
meta expr; 


idel expz.met; 
plot(data(:,1),data(:,4),t,navig(9,:));grid; 
xlabel(’seconds’);ylabel( ‘feet’ );pause; 

meta expz; 


idel expphi.met; 

plot (data(:,l) data(:75),t/ navig( 107:)); qr 107 
xlabel('seconds');ylabel('radians');pause; 
meta expphi; 


Idel exptheta.met; 
plot(ddatea(:,1),data(:,6),t,navegq( il), oud: 
xlabel(’seconds’);ylabel( ‘radians’ );pause; 
meta exptheta; 


222 


Idel exppsi.met; 

Fort (cata (:,1) gqata(:,7/),t,navig(12,:));gr1d; 
xlabel(’seconds’);ylabel(’radians’ );pause; 
meta exppsi; 


idel exppbias.met; 

plot(t,navig(13,:));/grid; 
xlabel('seconds');ylabel('radians/sec');pause; 
meta exppbias; 


!del expqbias.met; 

plot(t,navig(14,:));grid; 
xlabel('seconds');ylabel('radians/sec');pause; 
meta expqbias; 


!del exprbias.met; 

pilot (t,navigq(15,:));grid; 
xlabel('seconds');ylabel('radians/sec');pause; 
meta exprbias; 


idel expthbia.met; 

plo (t, navig(16,:))s;0rid; 
xlabel('seconds');ylabel('radians');pause; 
meta expthbia; 
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[*kk*k*k*k e k k k e e e e e e *k k k k k k k k x k k k x k k k k k k k k k k % X k x x X x 
* 


File AUV2.C 


C language source code for AUV2.MEX. Must be 
compiled within MEX subdirectory of MATLAB. 
It is used like a MATLAB function: 


function state = auv2(oldstate,inputs,dt) 


a FORTRAN based AUV2 model written 

by Prof. Fotis Papoulias and CDR David Warner, 
NPS, Monterey, CA. It is based on modified 
equations of motion from NSRDC Report 2510 
June, 1967. 


* 
* 
* 
* 
* 
* 
* 
* 
* Translated to C for MEX file use from SIM3D, 
* 
* 
* 
* 
* 
* 
* Called by AUVSIM7.M 

* 

* 


k * k e ee e UK k k k k x k k e e e e Kk e he e e e e e e e e e he e e he e e e he ke eek € / 


#include <math.h> 
Kinclude "auv2prm.h" 
#include "tcmex.h" 


double trapz( int n, double *a, const double *b ); 


int auv2( double *state, double *oldstate, double *inputs, 
double *dt ); 


/* 
* This is the code recognized by MATLAB 
x it calls function AUV2 


7 
void user fcn( int nlhs, Matrix *plhs[], int nrhs, Matrix 
*prhs[] ) 
{ 
double *oldstate, *inputs, *dt, *state; 


if (nrhs != 3) 

mex error("Must be three input arguments."); 
if (nlhs != 1) 

mexmerror( "Must bé one output arqument."); 


if (ROWS_IN(0) != 12 ¡¡ COLS_IN(O) != 1) 

mex error("Previous state vector not correct size."); 
if (ROWS_IN(1) != 5 jj COLS IN(1) !- 1) 

mex error("Input vector not correct size."); 
if (ROWS IN(2) ! 1 |j; COLS IN(2) != 1) 


mex error("Time interval must be a scalar."); 
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plhs[0] » create matrix(12,1,REAL); 
state - OUT(0); 

oldstate - IN(0); 

inputs - IN(1); 

dt = IN(2); 


auv2(state,oldstate,inputs,dt); 


* This code is the nonlinear model for NPS AUV II 


int auv2( double *state, double *oldstate, double *inputs, 
double *dt ) 
{ 
Int ES 
dnt flag = 0; 
double Dn ew DL roephacstheta..psi; 
double ars, drD, ds, ab. rom, delt; 
double uv, ssas, ssab, drss, drbs; 
double mass, heave, pitch, sway, yaw; 
double uch, er low: 
double tmpl EPA 
double vechi[15], vech2[15], vecv1[15], vecv2[15]; 
double fae ME e» 
double cos theta, sin theta, tan theta; 
double cos phi, sin phi, COS psi, SIN psi; 
u = oldstate[0]; 
V - oldstate[1]; 
w = oldstate[2]; 
p = oldstate[3]; 
q - oldstate[4]; 
E - oldstate[5]; 
phi - oldstate[9]; 
theta - oldstate[10]; 
psi - oldstate[11]; 
drs = inputs[0]; 
drb = inputs[1]; 
ds = inputs[2]; 
db = inputs[(3); 
rpm = inputs[4]; 
delt = *dt; 
mass = weight/g; 
G. P*/ 


E25 


/* 
^ 

/* 
i 


calculate the drag force, integrate the drag over 
the vehicle integrate using a 15 term trapezoidal 
numerical integration 


for (k=0; k<15; ++k) í 
tmpl = (v+xx[k]*r); 
tmp2 = (w-xx[k]*q); 
ucf = sqrt(fabs(tmpl*tmpl+tmp2*tmp2) ); 
if(ucf «X 1.0e-6) ( 
flag = 1; 
break; 
cflow = fabs((cdy*hh[k]*tmpl*tmpl + 
caz*br[k]*tmp2*tmp2) ucts: 
vechl[k] = cf£low*tmpl; 
vech2(k] = vechl(k]*xx[k); 
vecvl[k] = cflow*tmp2; 
vecv2[k] = vecvl[k]*xx[k]; 
} 
if (flag == 1) { 
heave = 0.0; 
pitch = 0.0; 
Sway = 0.0; 
yaw = 0.0; 
else { 
heave = trapz(i5,vecvl x); 
pitch = trapz(15,vecv2,xx); 
sway = trapz(15,vechl,xx); 
yaw = trapz(15,vech2,xx); 
heave = -rho*heave/2.0; 
pitch = Fenotpace/2Z.0; 
Sway = -rho*sway/2.0; 
yaw = -rho*yaw/2.0; 
} 
force equations 
common sub-expressions 
ssas = 0.0; 
ssab = 0.0; 
uv = u; 
drss = drs - ssas; 
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/ * 


* 


A 


/ * 


* 


27 


drbs = drb -ssab; 
cos theta = cos(theta); 
Sin theta = sin(theta); 
= tan(theta); 
= cos(phi); 
= sin(phi); 
cos psi = cos(psi); 
sin psi = sin(psi); 


if (phi == 0.0 ) (sin phi = 0.0 
if (theta == 0.0) (sin theta 
] 0.0 ) 


i} 
= 0; tan theta = 0.0; 4 
{Sin psi = 0. } 


0. 
0: 


f 


longitudinal force 


fp[0] = mass*v*r - mass*w*q + mass*xg*q*q 

+ mass*xg*r*r - mass*yg*p*q mass*zg*p*r 
XPP P PXT CEET E EXpE*p*r 
xwq*w*qtxvp*v*ptxvr*v*rt+u*q* (xqds*ds+xqdb*db) 
u*r*(xrdrs*drss + xrdrb*drbs) 
Xvv*v*v+xww*w*w + u*v*(xvdrs*drss+xvdrb*drbs ) 
u*w* (xwds*ds+xwdb*db) 
uv*uv* (xdsds*ds*ds+xdbdb*db*db 
xdrdr*(drss*drss+drbs*drbs)) 
(weight-boy)*sin theta + rpm*rpm*xprop - u*u*xres; 


LAR AA+ ++ 


lateral force 


fp[1] = -mass*u*r - mass*xg*p*q + mass*yg*r*r 
TS EZ A E a NOAA Oo Fi yr*u*r 
+ YVq*v*q + ywp*w*p 
+ ywrI*w*r + yv*u*v + yvw*v*w + ydrs*uv*uv*drss 
+ ydrb*uv*uv*drbs + (weight-boy)*cos _theta*sin _phi 
EEndgss w*pt mass*vg*p*b + Sway? 


normal force 


fp[2] 7» mass*u*q - mass*v*p - mass*xg*p*r - mass*yg*q*r 
llasccdgxumxprJnassezgsq*t*qeto2pp*p*por zpr*p*r 
ZIEL IDE A Cat EVE TV TE + zw*u*w 
ZvVv*v*vy + u*u*(zds*ds+zdb*db) 

(weight-boy)*cos theta*cos phi 

heave; 


+ + + + + 
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/* 
* roll moment 


s 


fp[3] = -iz*q*r F iy*q'r A Aa 
- iyz*r*r + ixz*p*q + mass*yg*u*q - mass*yg*v*p 
mass*zg*w*p + kpa*p*q + kgqr*g*r + Kkp*u*p + Kon 
kvq*v*q + kwp*w*p + kwr*w*r + kv*u*v + kvw*v*w 
(yg*weight - yb*boy)*cos theta*cos phi 
- (zg*weight - zb*boy)*cos theta*sin phi - 
mass"zg*u*r; 


++ | 


/* 
* pitch moment 
2 
fp[4] = -ix*p*r 4 Iz*p*r + 3xy*q*r - Xyz^*p*q - 3950200080 
+ ixz*r*r - mass*xg*u*q + mass*xg*v*p + mass*zg*v*r 
= mass*zg*w*q 4 mpp*p*p +mpr*pzr +mrr*r*rr'r ma mas 
+ mvp*v*p E MVE A EN AN 
+ u*u*(mds*ds+mdb*db) 
- (xg*weight-xb*boy)*cos theta*cos phi 
- (zg*weight-zb*boy)*sin theta + pitch; 
JE ak tae 
/* 
Z yaw moment 
ty, 
fp[5] = -ly*p*q + ix*p*q + ixy*p*p - ixy*q*q * iyz*p*r 
- lxz*q*r - mass*xg*u*r + mass*xg*w*p - mass*yg*v*r 
+ mass*yg*w*q + npa*p*a + nar*ta*r + Np uU p +T nr sum 
t nvq*v*q «4o nwbtw*p «CSnwBR*w*ros nv*uüu*vsLtonmvweu 
+ ndrs*uv*uv*drss + ndrb*uv*uv*drbs + (xg*weight 
- xb*boy)*cos theta*sin phi 
+ (yg*weight-yb*boy)*sin theta + yaw; 
/* 
* now compute the f(0-5) functions 
EA 
for (j=0; j<6; ++j) 
for (f[j]=0.0,k=0; k<6; ++k) 
f[)] += xmminv[j][k]*fp[k]; 
/* 
* the last six equations come from the kinematic 
* relations 
/* 
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* inertial position rates f(6-8) 


P 


f[6] » u*cos psi*cos theta -* v*(cos psi*sin theta* 
sin phi - sin psi*cos phi) -* w*(cos psi*sin theta* 
coSs philk Sin pSi*sin phil); 


f[7] = u*sin psi*cos theta + v*(sin psi*sin theta* 
si Iphifi COS ps tacos pri) + w"(sinipsi*sin theta* 
cos phi - cos_psi*sin_phi); 


f[8] » -u*sin theta + v*cos_theta*sin phi + 
wEeos theta*cos phi; 


y 
* euler angle rates f(9-11) 
E 
f[9]) = p * q*sin phi*tan theta -* r*cos phi*tan theta; 
f[10] s.q*cos phi - r*sin phài; 
f[l1l] = q*sin phi/cos theta + r*cos_phi/cos theta; 
/* 
* first order integration 
É / 


for (j=0; 3<12; 3++) 
state[j] = oldstate[j] + delt * £[j]; 


return 0; 


} 
/* 


* This function performs a trapezoidal integration 
E / 
double trapz( int n, double *a, const double *b ) 
WME nl, k; 
double y; 
nl = n-1; 
for (y = 0.0, k=0; k<nl; k++) 
y += (a[k] + a[k+1])*(b[k+1]-b[K])/2; 


Return y; 
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File AUV2AB.C 


C language source code for AUV2AB.MEX. Must be 
compiled within MEX subdirectory of MATLAB. 
It is used like a MATLAB function: 


function [A,B] = auv2AB(state,inputs) 


Based on Taylor series expansion of the equations 
of motion in AUV2.C 


Called by AUVSIM7.M 


* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 


ok k k k K k H H H k k x k k e k e e e e k k K K e he ke Sec ke e he e e e ke e ke kk kk x / 


include «math.h» 
#include "tcmex.h" 
#include “auv2prm.h" 


void makeab( double *A, double *B, double *state, double 
PUES 


* 
* This section of code is recognized by MATLAB 
* It calls function MAKEAB 


* 


void user fcn( int nlhs, Matrix *plhs[], int nrhs, Matrix 
*prhs[] ) 


double *state, *inputs, *a, *b; 


if (nrhs != 2) 

mex_error("Must be two input arguments"); 

if (nlhsit- 225 

mex error("Must be two output arguments"); 

if (ROWS IN(0) != 12 || COLS IN(0) !- 1) 
mex_error("Initial state vector must have 12 states"); 
if (ROWS-EN(1) != 5 u! COEN IO 


mex error(" Input vector must have 5 elements"); 


plhs[0] create matrix( 12 T 2FREAL); 
plhs[1] create matriz=(12,5 REAL); 
a = OUT(0); 

b = OUT(1); 

state - IN(0); 

inputs - IN(1); 


makeab(a,b,state,inputs); 
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| 
/* 


* This code makes the linearized model. 


Pi 


void makeab( double *A, double *B, double *state, double 
*inputs ) 


"Ost posi. es 

usuble a[12][12], b[6][5]; 

double aa[(12][12],bb[12][5]; 

double u0, v0, w0, pO, qO, r0, phiO, thetaO, psiO0; 
double mass; 

double drs, drb, ds, db, rpm; 

double cos theta, sin theta, tan theta; 

double cos phi, sin phi, cos psi, sin psi; 


/* assign some common variable names */ 


u0 - state[0]; 
vO = state[1]; 
wO = state[2]; 
pO = state[3]; 
qO = state[4]; 
Pum) State[ 5]; 


ph10 = state[9]; 
theta0 = state[10]; 
Psi0 = state[11]; 


drs = inputs[0]; 
drb = inputs[1]; 
ds = inputs[2]}; 
db = inputs[3]; 
rpm = inputs[4]; 


cos theta 
sin theta 


cos(theta0); 
sin(theta0); 


tan theta - tan(theta0); 

cos phi = cos(phi0); 

Sin phi = sin(phi0); 

cos psi - COS(psi0); 

sin psi = sin(psi0); 

if (theta0 == 0.0) [ sin theta = 0.0; tan theta = 0;) 


if (phi0 == 0.0) sin phi = 0.0; 
if (psi0 == 0.0) sin psi = 0.0; 


mass = weight/g; 


TSL 


/* initialize a,b, ada, and boa 


for 3-0; J 27m) 
for (k=0; k<12; Fb O) 
a[j)[ÉF] — O 0, 


for (j=0; 3<6; j++) 
for (k=0; k<5; k++) 
b[j](k] = 0.0; 


fore(g20; 0g LE 
tor (k=0;/k<12; Tkr) 
aa[j][k] » 0.0; 


for (3j=0; 3<12; 3++) 
for (k=0; k<5; k++) 
bb[j][k] = 0.0; 


/* Build the 12x12 a matrix */ 
/* d(long. force)/dx */ 


a[0][0]= q0*(xqds*ds+xqdb*db)+r0* (xrdrs*drstxrdms>- dro 
v0O* (xvdrs*drs+xvdrb*drb) +w0* (xwds*ds+xwdb*db) + 
2*u0* (xdsds*ds*ds+xdbdb*db*db+ 
xdrdr* (drs*drs+drb*drb) )-2*xres*u0; 
a[0][1]= mass*r0+xvp*p0+xvr*r0+2*xvv*v0+ 
uO*(xvdrs*drs*xvdrb*drb)- 
-mass*q0+xwgq*q0+2*xww*w0+u0* (xwds*ds+xwdb*db) ; 
-mass*yg*q0-mass*zg*r0+2*xpp*p0+xpr*r0+xvp*v0; 
-mass*w0+2*mass*xg*q0—mass*yg*p0+2*xqgq*q0+ 
xwq*w0+u0* (xqds*ds+xqdb*db) ; 
a[0][5]= mass*v0+2*mass*xg*r0-mass*zg*p0+2*xrr*r0+xpr*p0+ 
xvr*v0+u0* (xrdrs*drs+xrdrb*drb); 
a[0][10]=-(weight-boy)*cos theta; 


w vw 
ooo 
l HI Il 


2] 
3] 
4] 


ae ee 


[0] 
[9] 
[0] 


/* d(lat. force)/dx */ 


a[1](0]=-mass*r0+yp*p0+yr*r0+yv*v0+2*u0*(ydrs*drs+ydrb*drb); 
a[1)(1]= yvq*q0+yv*u0+yvw*w0; 

a[1][2]= mass*pO0+ywp*pO0+ywr*r0+yvw*v0; 

a[1]([3]= 
mass*w0-mass*xg*q0+2*mass*yg*p0+ypq*q0+yp*u0+ywp*wO; 
a[1][4]=-mass*xg*p0-mass*zg*r0+ypq*p0+ygr*r0+yvq*v0; 
a[1][5]- 
-mass*u0+2*mass*yg*r0-mass*zg*q0+ygr*q0+yr*u0+ywr*w0; 
a[1][9]= (weight-boy)*cos _theta*cos phi; 
a[1][10]2-(weight-boy)*sin theta*sin phi; 
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/* d(norm. force)/dx */ 


mass*q0+zq*q0+zw*w0+2*u0*(zds*ds+zdb*db); 
-mass*p0+zvp*p0+zvr*r0+2*zvv*v0; 

zw*u0; 
-mass*v0-mass*xg*r0+2*mass*zg*p0+2*zpp*p0+ 
zZpr*rI0+*zvep*v0; 

4]= mass*u0-mass*yg*r0+2*mass*zg*q0+zq*u0; 

5 }=-mass*xg*p0-mass*yg*q0+zpr*p0+2*zrr*r0+zvr*v0; 
9 

1 


now Hl Il 


]--(weight-boy)*cos theta*sin phi; 
0]--(weight-boy)*sin theta*cos phi; 


/* d(roll moment)/dx */ 


= mass*yg*q0+mass*zg*r0+kp*p0+kr*r0+kv*v0; 
=-mass*yg*p0+kvq*q0+kv*u0+kvw*w0; 
=-mass*zg*p0+kwp*p0+kwr*r0+kvw*v0; 
-ixy*r0+ixz*q0-mass*yg*v0-mass*zg*w0+ 
kpq*q0+kp*u0+kwp*w0; 
a[3][(4)]=-1z*r0+iy*r0+2*iyz*q0+ixz*p0+mass*yg*u0+ 
kpq*p0+kqr*r0+kvq*v0; 
a[3][5]=-iz*q0+iy*q0-ixy*p0-2*iyz*r0+mass*zg*u0+ 
kar*q0+kr*u0+kwr *w0; 
a[3][9]=-(yg*weight-yb*boy)*cos_theta*sin_phi- 
(zg*weight-zb*boy)*cos theta*cos phi; 
a[3][10]2-(yg*weight-yb*boy)*sin theta*cos phi- 
(zg*weight-zb*boy)*sin theta*sin phi; 


/* d(pitch moment)/dx * / 


a[4][0]2-mass*xg*q0-4*mq*q0-4*mw*w0-2*u0* (nds*ds-4*mdb*db); 
a[4][1]= mass*xg*p0+mass*zg*r0+mvp*p0+mvr*r0+2*mvv*v0; 
a[4][2]=-mass*zg*q0+mw*u0; 
a[4][3]=-ix*r0+iz*r0-iyz*q0-2*ixz*p0+mass*xg*v0+ 
2*mpp*p0+mpr*r0+mvp*vo0; 
a[4][4]= ixy*r0-iyz*p0-mass*xg*u0-mass*zg*w0+mq*u0; 
a[4][5)]=-1x*p0+iz*p0+ixy*q0+2*ixz*r0+mass*zg*v0+ 
Wp p07 Muse Omiya «v0, 
J= (xg*weight-xb*boy)*cos theta*sin phi; 
0]=(xg*weight-xb*boy)*sin_theta*cos phi- 
(zg*weight-zb*boy)*cos theta; 


e d(yaw moment)/dx */ 
a[5][0]=-mass*xg*r0+np*p0+nr*r0+nv*v0+ 


2*u0* (ndrs*drs+ndrb*drb) ; 
a[5][1)]=-mass*yg*r0+nvq*q0+nv*u0+nvw*w0; 
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a[5][2]= mass*xg*p0+mass*yg*q0+nwp*pO0+nwr*r0+nvw*v0; 

a[5][3]=-iy*q0+ix*q0+2*ixy*p0+iyz*r0+mass*xg*w0+ 
npq*q0+np*u0+nwp*w0; 

a[5][4]=-iy*p0+ix*p0-2*ixy*q0-ixz*r0+mass*yg*w0+ 
npa*pO-*ngr*rO*nvq*vo; 

a[5][5]= iyz*p0-ixz*q0-mass*xg*u0-mass*yg*v0+ 
ngr*q0+nr*u0+nwr*w0; 


a[5][9]= (xg*weight-xb*boy)*cos_theta*cos phi; 

a[5][10]2-(xg*weight-xb*boy)*sin theta*sin phi- 
(yg*weight-yb*boy)*cos theta; 

/* d(inertial position X)/dx * / 

a[6][0]» cos psi*cos theta; 

a[6](1]2» cos psi*sin theta*sin phi-sin psi*cos phi; 

a[6][2]2» cos psi*sin theta*cos phi-sin psi*sin phi; 

a[6][9]= v0*(cos psi*sin theta*cos phi+sin psi*sin phi)+ 


wO*(-cos psi*sin theta*sin phi+sin psi*cos phi); 
a[6][10]=-u0*cos psi*sin theta+v0*cos psi*cos theta* 
sin phi+w0*cos psi*cos_theta*cos phi; 
a[6][(11]2-uO*sin psi*cos theta-vO0*(sin psi*sin theta*sin phi 
+cos psi*cos phi)+ 
wO*(-sin psi*sin theta*cos phi+cos psi*sin phi); 


/* d(inertial postion Y)/dx */ 


a(7][0]2» sin psi*cos theta; 

a[7][1]2» sin psi*sin theta*sin phi-*cos psi*cos phi; 
a[7][2]» sin psi*sin theta*cos phi-cos psi*sin phi; 
a[7][9]2» vO*(sin psi*sin theta*cos phi-cos psi*sin phi)- 


wO*(sin psi*sin theta*sin phi-*cos psi*cos phi); 
a[7][10]=-u0*sin psi*sin theta+v0*sin psi*cos theta*sin phi+ 
wO*sin psi*cos theta*cos phi; 
a[7](11]= u0*cos psi*cos theta+ 
vO*(cos psi*sin theta*sin phi- 
sin psi*cos phi)+ 
wO*(cos psi*sin theta*coS"phitsin psi*sin phos 


/* d(depth)/dx * / 


a[8][0]2-sin theta; 

a[8][(1]2» cos theta*sin phi; 

a[8][2]= cos_theta*cos_phi; 

a[8][9]= v0*cos_theta*cos phi-w0*cos _theta*sin _phi; 

a[8][10]2-u0*cos theta-vO*sin theta*sin phi- 
wO*sin theta*cos phi; 
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(roll angle) /dx x 


a[9][3]-71; 

a[9][4]=sin phi*tan theta; 

ANS cos phi*tan theta; 
a[9][9]=q0*cos phi*tan theta-r0*sin_phi*tan theta; 
a[9][10]=(q0*sin phi+r0*cos phi)/cos_theta*i1/cos theta; 


/* d(pitch angle) /dx * / 


a{10][4]=cos_phi; 
a[10][5]=-sin phi; 
a[10][9]=-q0*sin_phi-r0*cos_phi; 


/* d(yaw angle)/dx * / 

a[11][4]2sin phi/cos theta; 

a[11][5]2cos phi/cos theta; 
a[11][9]2-q0*cos phi/cos theta-rO0*sin phi/cos theta; 
a[11][(10]=(q0*sin phi+r0*cos phi)/cos theta*tan theta; 
/* Build the 12x5 b matrix */ 


/* d(long force)/d(inputs)y */ 


b[0][0]= xrdrs*u0*r0+xrdrs*u0*v0+2*u0*u0*xdrdr*drs; 
Boyt lj= xrdrb*u0*r0+xrdrb*u0*v0+2*u0*u0*xdrdr*drb; 
b[0][2]= u0*q0*xgds+u0*w0*xwds+2*u0*u0*xdsds*ds; 
b[0][3]= u0*q0*xgdb+u0*w0*xwdb+2*u0*u0*xdbdb*db; 
b[0][4]= 2*xprop*rpm; 


/* d(lat force)/d(inputs) * 


ydrs*u0*u0; 
ydrb*u0*u0; 


/* d(normal force)/d(inputs) * / 
= u0*u0*2ds; 
= u0*u0*zdb; 
/* d(roll moment)/d(inputs) * / 


b[3][3]= 0; 
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/* d(pitch moment)/d(inputs) */ 


b[4][2]= u0*u0*mds; 
b[4][3]» u0*u0*mdb; 


/* d(yaw moment)/d(inputs) */ 


b[5][(0]= ndrs*u0*u0; 
b[5](1]» ndrb*u0*u0; 


/* Multiply the appropriate parts of both by the inverted 
ts mass matrix 
* 
* inv(mass matrix)*df/dx 


a 


Lora =0 60 MEL 
for (IU ues) 
for (k=0; k<6; k++) 
^ aa[i][j] *- xmminv[i][k]*a(k][j3]; 
x inv(mass matrix)*df/dz 


E 


for (1=0; 1<6; i++) 
for (j=6; j<12; JTF) 
for (k=0; k<6; k++) 
aaf il] o AS e 


for (i=6; i<12; i++) 
for (j=0; 3<12; j++) 
aa[iJ[j] = a[1](3]; 


/* 
* inv(mass matrix) *df/du 
x 
for (150; i6; 1t) 
for (j=0; j<5; j++) 
for (k=0; k<6; k++) 
bb(1][3] += xmminv(i][(k]*b[k][3]; 


* reorganize the matrices for use by Matlab 
* which stores matrices columnwise vice rowwise 


for (j=0; 3<12; j++) 
for (1=0; i<12; i++) 
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Af itI2*j] = aaf iliyi, 


RDO; j5; j++) 
for (i=0; i<12; i++) 
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f***kkkkkdk hk hk h k k k Ye de I DD SAID K HACHE AA EE e Ie 
* 


File AUV2PRM.H 


* 

* 

* This file contains all of the parameter 

* coefficients used by the file AUV2.C 

* and AUV2AB.C. 

* The format is identical to that in MODELPRM.H 
* which contains parameters for the Swimmer 

* Delivery Vehicle 
* 

* 

* 

* 

* 

* 


These parameters were determined by 
Prof. Fotis Papoulias and CDR David Warner, NPS 
Monterey, CA. 


ERRATA AA AE O | 


/* 
* define some constants for use in expressions 


5 


#define trm2 1.94/2*87.625/12.0*87.625/12.0 

#define trm3 1.94/2*87.625/12.0*87.625/12.0*87.625/12.0 

#define trm4 
1.94/2*87.625/12.0*87.625/12.0%*87 .625/12.0*87 16257 eee 

#define trm5 
1.94/2*87.625/12.0%87.625/12.0*675 6257 1250-3 7 0257 
12.0*87.625/12.0 

#define lngth 87.625/12.0 

#define urpm 2.5/550.0 


/* 
* mass characteristics of the vehicle 
uA 
const double 
weight = 435.0, boy = 435.0, vol = 200.0, 
xg = 0.125/12.0, yg ~- 0.0, zg = 0.05, 
xb = 0.125/12.0 "cb = 0.0, ix = 247 
iy - 42.0, iz - 45.0, ixz = 0.0, 
1yz = 0.0, ixy = 0.0, yb = 0.0, 
l = 91.625 20 0 FNOwE 1.94, g = 32.2; 
cd0 = 0.015, 
cdy = 0.5, cdz = 072767; rpm0= 550.0, u0 = 2.5% 
xrs = -0.377*1ng h xr = 0.283*ingth; 
j= 
* longitudinal hydrodynamic coefficients 
* 
/ 
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const double 


Xpp 
Xpr 
xvp 
xqdb 
xvv 
xvdrb 
xdsds 
xdbdb 
xdrdr 
xres 


0.0 8 xq == 0.0, Xrr = -0.017355*trm4; 
0.0, xudot = -2.82e-3*trm3, xwq = 00, 
0.0, xvr = 04 02 xqds = 0.0, 
0.0, xrdrs = 0.0, xrdrb = 0.0, 
-4.019e-2*trm2, xww = 0.0, xvdrs = 0.0, 
0.0, xwds = 0.0, xwdb = 0.0, 


-2.345e-3*0.417*trm2, 
-2.345e-3*0.417*trm2, 
-2.345e-3*0.417*trm2, 
070U5*CEM27 xprop 


0.015*trm2*(urpm)*(urpm); 


/* 
* lateral hydrodynamic coefficients 
f 
const double 
ypdot = 0.0, yrdot = -1.78e-3*trm4, ypq = 0.0, 
yqr = 0.0, yvdot = -3.43e-2*trm3, yp = 0.0; 
yr = 1.187e-2*trm3, yvq = 0.0, ywp = 0.0, 
Ywr = OSO, yv = -3.896e-2*trm2, 
yvw = 0.0, ydrs = 2.345e-2*trm2, 
ydrb = 2.345e-2*trm2; 
/* 
* normal hydrodynamic coefficients 
nA 
const double 
zqdot = -2.53e-3*trm4, zpp = 0.0, zpr = 0.0, 
zrr = 0.0, zwdot = -9.34e-2*trm3, zq = -7.013e-2*trm3, 
ZVp = 00 /* zyr = 0.05 zw = -1.5678e-1*trm2, 
ZVV = 0.0, zds = -2.345e-2*trm2, zdb =-2.345e-2*trm2; 
/* 
* roll hydrodynamic coefficients 
wd 
const double 
kpdot = -2.4e-4*trm5, krdot = 0.0, kpq = 0.0, 
kqr = DOS kvdot = 0.0, kp = -5.4e-3*trmí, 
kr = O. 0, kvq = 0.0, kwp = 0.0, 
kwr = 0.0, kv = 0.0, kvw = 0.0; 
/* 
* pitch hydrodynamic coefficients 
7 
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const double 


mqdot = -6.25e-3*trm5, mpp = 0704, mpr =T070;, 
mrr = O70), mwdot = -2.53e-3*trm4, 

mq = -3.565e-2*trm4, mvp = 0.0, mvr = 0.0, 
mw = 5.122e-2*trm3, mvv = 0.0; 

mds -0.377*1ngth*2.345e-2*trm2, 


mdb 0.283*1ngth*2.345e-2*trm2; 
/* 
À yaw hydrodynamic coefficients 
= 
const double 
npdot = 0.0, nrdot = -4.7e-4*trm5, npq = 0.0; 
nqr = 0.0, nvdot = -1.78e-3*trm4, np = 0702 
nr = -1.022e-2*trm4, nvq = 0.0, nwp = 0.0, 
nwr = 0.0, nv = -7.69e-3*trm3, nvw = 0.0. 
ndrs = -0.377*1ngth*2.345e-2*trm2, 
ndrb = 0.283*1ngth*2.345e-2*trm2; 
* 
x define length fractions terms for gauss quadrature 
T 
const double 
x[] = {-43.9/12.0, -39.2/12.0, -35.2/12.0, -31.2/12.07 
-27.2712.0,%-10.;0712.,0;, 0:07 10 0712. 0) 
26.8/12.0, 32.0/12.0, 37.8/12.0, 40.87712 
42.3/12.0, 4323/1220, 23 7 12 0) 
/ * 
* define the breadth br and height hh terms 
e 


const double 
hh[] = 1 0.0/12.0, 2.771220 25827 127 OG O 
10.1/12.0, [1077712707017512 0790807777 127207 
10.1/12.0, 9.6/12.0 776/12 060 65 56 1270; 


4:2/12.0,4 293/1120 00€, 
br[] 2.1516.5/12:50, 16.5/7/12.0 216. 57/12 0 pl 5 l POP 
16.5/12.0, 16.5/12.0 M6 5 P 0 LOS On 
16.5/12:-07 15 75/12.0 212.45712 0 0 5712 05 
7.0/1270 m4%0y712 0 8050 1 
/ 
* assemble inverted mass matrix 
* this matrix was calculated previously using the above 
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* coefficients and the equations of motion 


a 


const double 
xmminv[6][6] = ( 


CMS O 389026554 9:0, =2:7774e-4, 0.0 1, 
{ 0.0, 3.8558e-2, 0.0, 3.4574e-3, 0.0, -3.5748e-3 }, 
{ 3.8922e-5, 0.0, 2.0616e-2, 0.0, -8.3980e-4, 0.0 }, 
[ 0.0, 3.4574e-3, 0.0, 1.3306e-1, 0.0, -3.2055e-4 ), 
[-2.7774e-4, 0.0,-8.3980e-4, 0.0, 5.9927e-3, 0.0 }, 
Is 65 0 C 3) 2054e"4, “0.0, 1.8692e-2 ) 


1; 
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% % % % % % % % $ % % % $ $ % $ $ $ $ % $ $ $ % $ $ % $ $ % $ $ $ $ $ $ $ % $ % % $ $ $ % $ 

% 

% File MAKEK.M 

% 

% 

% Function which makes the piecewise constant 

3 Extended Kalman filter gain matrices for various 
$ values of forward speed, u, and pitch angles, 
% theta. Assumes that other states are zero to 
% keep the number of different K matrices to a 

% minimum. Returns a block row matrix of steady 
% state K matrices 

% 

$ Calls DLQE.M, AUV2AB.MEX, 

% 

% Called by AUVSIM7.M 

% 

% % % % $ $% $ $ $ $ $ $ $ % % % $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ $ % % % 

function allK = makeK(uRange,thtRange,Q,R,dt) 

allK = []; 


u = uRange; 
theta = thtRange; 


and 
550 rpm gives 2.5 feet/sec 


$ calculate rpm range corresponding to the u range using 
$ known rpm to thrust relationship and drag to speed 
$ relationships: 

% 

% thrust = drag (in steady state) 

% thrust - constl*rpm^2 

$ drag - const2*u^2 

$ 

% 

% 

% 


rpm = u*5507/275; 
% shell for measurement matrix C: 
C=[ 0 


p 


q 
É 


depth 

depth dot 

Theta (pitch + bias 
Psi (heading gyro) 


+ biases, 


| a 








OOOOoOoo 

OOOoOoOoOcoOo 
S) (S) (SY (S) (S) (S). (D. 
2) (S) (S) (S) (œS) (œ) (== 
O O O O O = O 
OOO OIR OIO 
O O OF O OO 
OOOOooOoo0oo0 
O = O O O O O 
KH-OO00 0 © 
OOoOooOooconr 
@ O O O O OQO 
CDF 


í 


oreo oc od © 
oe oe oe ge ge oe oe 


° 
f 
í 
° 
/ 
, 
° 
í 
e 
í 


$ loop through both 


£ 


and theta to calculate K, 
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% the Extended Kalman filter gain matrices 


for uIndx = l:length(u) 


Pout =[0;0;0;0;rpm(ulndaxp; 


mom thindx = 1: length(theva) 


% 


E5isauv2ab([u(uIndx);zeros(9,l1);theta(thindx);0],input); 


Setup the full 12 state linearized model 


$ delete from 'a' and 'b' elements having to do 
$ with X and Y to remove poles at origin to 
$ help stabilize 
$ the Extended Kalman filter 
a = al. 651 6) a(1:6;,9:12); 
dq 9.12 1 653 (9:12; ;9:12)]; 
D = MBE :); 
DINOS IZA, 
% add gyro bias terms as constant states 
$ with random walk 
a = [a zeros(10,4); 
zeros(4,14)]; 
b = [b;zeros(4,5)]; 
$ disturbance input matrix 
b2=sign(Q); 
% convert to discrete time 


[Phi,Gam2]=c2d(a,b2,dt); 


+ Recalculate the depth dot row of the C matrix 

% around u and theta only 

C(5,1) = -sin(theta(thIndx) ); 

C(5,3) = cos(theta(thIndx) ); 

C(5,9) = -u(uIndx)*cos(theta(thIndx)); 

% calculate the Extended Kalman filter gain matrix for 
% üu & theta values, & append it to form a matrix made 
$ of gain matrices 

$ DLOE solves the discrete time Riccati equation 

$ to find the steady state Kalman filter gain for a 

$ given linear system 


allK - [allK,real(dlqe(Phi,Gam2,C,Q,R))]; 


end; 


end; 
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EEEEEEEEEEEEEREEETEEEEEEEEEEEEEEEETEEELEEEE EES 
$ 
File GETK.M 


$ 
$ 
% Function which compares estimated speed and 

% estimated pitch to the nominal speed and pitch 

% vectors to determine which of the steady state 

% Kalman filter gain matrices to use in the 

% piecewise constant approximation for the Extended 
% Kalman filter 

% 

% 

% 

% 


Called by AUVSIM7.M 
% % % % % % % % % % % % % % % % % $ % % % % % $ $ % % % $ $ % $ $ % % $ $ % % $ $ % % % % 
function K = getK(allK,u,theta,uRange,thRange) 


% determine index of the nominal speed and pitch which 
% is closest to the actual speed and pitch 


[error,uIndx] = min(abs(u-uRange) ); 
[error,thIndx] = min(abs(theta-thRange) ); 


allK was made by MAKEK.M and therefore it is 

a block row matrix of K matrices calculated over the 
the range of u values in uRange and the range of theta 
values in thRange. Theta is the most rapidly changing 
index so allK has the following format: 


allK =... 
[K(uRange(1),thRange(1)),K(uRange(1),thRange(2)),... 
K(uRange(last),thRange(last-1)),... 
K(uRange(last),thRange(last))] 


so allK has: 

(number of states in Extended Kalman filter) rows & 
(length(uRange)*length(thRange)*(number of measurements)) 
columns 


oo ge ge ge ge oe ge ge ge ge ge ge oe oe oe 


[m,n] = size(allK); 
Kwidth = n/length(uRange)/length(thRange); 


% so the proper K to use is 
Indx = 1 + Kwidth*((uIndx-1)*length(thRange) + (thIndx-1)); 


K = allK(:,Indx:(Indx+Kwidth-1)); 
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% % % % $ % % % % % $ $ $ % $ $ $ % $ $ % $ $ % $ $ % % % $ % % % % $ $ $ % % $ $ % % $ $ $ 


% 

$ File GETMEAS5.M 

% 

% formats actual recorded measurement data for 

% navigator 

% 

% Ver.4 using recorded data vice simulated data 

% 

% Ver.5 use pitch gyro directly rather than through 
% Simulated accelerometers 

% 


% % % % % % % % $ % % $ % $ % % % $ $ $ $ $ % $ $ $ $ $ $ $ $ $ $ $ $ $ $ % $ $ % $ $ $ $ % $ 
function [anglerate,Z,Zdot,pitch,hdg]=getmeas5(data,Zold,dt) 


% depth, Z, measurement 
Z = data(4); 


% time rate of change of depth 
Zdot = (Z - Zold)/dt; 


$ rate gyro readings 

anglerate(1,1)=data(8); 
anglerate(2,1)=data(9); 
anglerate(3,1)=data(10); 


pitch = data(6); 
hdg = data(7); 
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% % % % % % % $ % $ $ $ $ $ % % $ $ $ % $ $ $ $ $ % % % % % % % % % % % % % $ $ $ % $ % % $ 
% 
File MAKMEAS3.M 


subroutine to transform the observer states 
. into measurements 


% 
% 
% 
% 
% 
% Called by AUVSIM7.M 

% 

% % % % % $ % % % % $ % % $ $ % % $ $ $ $ % $ $ % $ $ % % % $ $ % $ $ % % $ $ $ $ % $ % % $ 
function [modmeas ]=makmeas3(new,old,dt) 


modmeas = zeros(7,1); 


modmeas(1) = new(4) + new(13); $ rate gyro 
modmeas(2) = new(5) + new(14); % with bias added) 
modmeas(3) = new(6) + new(15); 

modmeas(4) = new(9); % depth 
modmeas(5) = (new(9)-01d(9))/dt; % depth dot 
modmeas(6) = new(11) + new(16); 

modmeas(7) = new(12); 
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